//  (C) Copyright Anton Bikineev 2014
//  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_MATH_TOOLS_RECURRENCE_HPP_
#define BOOST_MATH_TOOLS_RECURRENCE_HPP_

#include <boost/math/tools/config.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/math/tools/fraction.hpp>
#include <boost/math/tools/cxx03_warn.hpp>
#include <boost/math/tools/assert.hpp>

namespace boost {
   namespace math {
      namespace tools {
         namespace detail{

            //
            // Function ratios directly from recurrence relations:
            // H. Shintan, Note on Miller's recurrence algorithm, J. Sci. Hiroshima Univ. Ser. A-I
            // Math., 29 (1965), pp. 121 - 133.
            // and:
            // COMPUTATIONAL ASPECTS OF THREE-TERM RECURRENCE RELATIONS
            // WALTER GAUTSCHI
            // SIAM REVIEW Vol. 9, No. 1, January, 1967
            //
            template <class Recurrence>
            struct function_ratio_from_backwards_recurrence_fraction
            {
               typedef typename boost::remove_reference<decltype(boost::math::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;
               typedef std::pair<value_type, value_type> result_type;
               function_ratio_from_backwards_recurrence_fraction(const Recurrence& r) : r(r), k(0) {}

               result_type operator()()
               {
                  value_type a, b, c;
                  boost::math::tie(a, b, c) = r(k);
                  ++k;
                  // an and bn defined as per Gauchi 1.16, not the same
                  // as the usual continued fraction a' and b's.
                  value_type bn = a / c;
                  value_type an = b / c;
                  return result_type(-bn, an);
               }

            private:
               function_ratio_from_backwards_recurrence_fraction operator=(const function_ratio_from_backwards_recurrence_fraction&);

               Recurrence r;
               int k;
            };

            template <class R, class T>
            struct recurrence_reverser
            {
               recurrence_reverser(const R& r) : r(r) {}
               boost::math::tuple<T, T, T> operator()(int i)
               {
                  using std::swap;
                  boost::math::tuple<T, T, T> t = r(-i);
                  swap(boost::math::get<0>(t), boost::math::get<2>(t));
                  return t;
               }
               R r;
            };

            template <class Recurrence>
            struct recurrence_offsetter
            {
               typedef decltype(std::declval<Recurrence&>()(0)) result_type;
               recurrence_offsetter(Recurrence const& rr, int offset) : r(rr), k(offset) {}
               result_type operator()(int i)
               {
                  return r(i + k);
               }
            private:
               Recurrence r;
               int k;
            };

         }  // namespace detail

         //
         // Given a stable backwards recurrence relation:
         // a f_n-1 + b f_n + c f_n+1 = 0
         // returns the ratio f_n / f_n-1
         //
         // Recurrence: a functor that returns a tuple of the factors (a,b,c).
         // factor:     Convergence criteria, should be no less than machine epsilon.
         // max_iter:   Maximum iterations to use solving the continued fraction.
         //
         template <class Recurrence, class T>
         T function_ratio_from_backwards_recurrence(const Recurrence& r, const T& factor, std::uintmax_t& max_iter)
         {
            detail::function_ratio_from_backwards_recurrence_fraction<Recurrence> f(r);
            return boost::math::tools::continued_fraction_a(f, factor, max_iter);
         }

         //
         // Given a stable forwards recurrence relation:
         // a f_n-1 + b f_n + c f_n+1 = 0
         // returns the ratio f_n / f_n+1
         //
         // Note that in most situations where this would be used, we're relying on
         // pseudo-convergence, as in most cases f_n will not be minimal as N -> -INF
         // as long as we reach convergence on the continued-fraction before f_n
         // switches behaviour, we should be fine.
         //
         // Recurrence: a functor that returns a tuple of the factors (a,b,c).
         // factor:     Convergence criteria, should be no less than machine epsilon.
         // max_iter:   Maximum iterations to use solving the continued fraction.
         //
         template <class Recurrence, class T>
         T function_ratio_from_forwards_recurrence(const Recurrence& r, const T& factor, std::uintmax_t& max_iter)
         {
            boost::math::tools::detail::function_ratio_from_backwards_recurrence_fraction<boost::math::tools::detail::recurrence_reverser<Recurrence, T> > f(r);
            return boost::math::tools::continued_fraction_a(f, factor, max_iter);
         }

         // solves usual recurrence relation for homogeneous
         // difference equation in stable forward direction
         // a(n)w(n-1) + b(n)w(n) + c(n)w(n+1) = 0
         //
         // Params:
         // get_coefs: functor returning a tuple, where
         //            get<0>() is a(n); get<1>() is b(n); get<2>() is c(n);
         // last_index: index N to be found;
         // first: w(-1);
         // second: w(0);
         //
         template <class NextCoefs, class T>
         inline T apply_recurrence_relation_forward(const NextCoefs& get_coefs, unsigned number_of_steps, T first, T second, long long* log_scaling = 0, T* previous = 0)
         {
            BOOST_MATH_STD_USING
            using boost::math::tuple;
            using boost::math::get;

            T third;
            T a, b, c;

            for (unsigned k = 0; k < number_of_steps; ++k)
            {
               tie(a, b, c) = get_coefs(k);

               if ((log_scaling) &&
                  ((fabs(tools::max_value<T>() * (c / (a * 2048))) < fabs(first))
                     || (fabs(tools::max_value<T>() * (c / (b * 2048))) < fabs(second))
                     || (fabs(tools::min_value<T>() * (c * 2048 / a)) > fabs(first))
                     || (fabs(tools::min_value<T>() * (c * 2048 / b)) > fabs(second))
                     ))

               {
                  // Rescale everything:
                  long long log_scale = lltrunc(log(fabs(second)));
                  T scale = exp(T(-log_scale));
                  second *= scale;
                  first *= scale;
                  *log_scaling += log_scale;
               }
               // scale each part separately to avoid spurious overflow:
               third = (a / -c) * first + (b / -c) * second;
               BOOST_MATH_ASSERT((boost::math::isfinite)(third));

               swap(first, second);
               swap(second, third);
            }

            if (previous)
               *previous = first;

            return second;
         }

         // solves usual recurrence relation for homogeneous
         // difference equation in stable backward direction
         // a(n)w(n-1) + b(n)w(n) + c(n)w(n+1) = 0
         //
         // Params:
         // get_coefs: functor returning a tuple, where
         //            get<0>() is a(n); get<1>() is b(n); get<2>() is c(n);
         // number_of_steps: index N to be found;
         // first: w(1);
         // second: w(0);
         //
         template <class T, class NextCoefs>
         inline T apply_recurrence_relation_backward(const NextCoefs& get_coefs, unsigned number_of_steps, T first, T second, long long* log_scaling = 0, T* previous = 0)
         {
            BOOST_MATH_STD_USING
            using boost::math::tuple;
            using boost::math::get;

            T next;
            T a, b, c;

            for (unsigned k = 0; k < number_of_steps; ++k)
            {
               tie(a, b, c) = get_coefs(-static_cast<int>(k));

               if ((log_scaling) &&
                  ( (fabs(tools::max_value<T>() * (a / b) / 2048) < fabs(second))
                     || (fabs(tools::max_value<T>() * (a / c) / 2048) < fabs(first))
                     || (fabs(tools::min_value<T>() * (a / b) * 2048) > fabs(second))
                     || (fabs(tools::min_value<T>() * (a / c) * 2048) > fabs(first))
                  ))
               {
                  // Rescale everything:
                  int log_scale = itrunc(log(fabs(second)));
                  T scale = exp(T(-log_scale));
                  second *= scale;
                  first *= scale;
                  *log_scaling += log_scale;
               }
               // scale each part separately to avoid spurious overflow:
               next = (b / -a) * second + (c / -a) * first;
               BOOST_MATH_ASSERT((boost::math::isfinite)(next));

               swap(first, second);
               swap(second, next);
            }

            if (previous)
               *previous = first;

            return second;
         }

         template <class Recurrence>
         struct forward_recurrence_iterator
         {
            typedef typename boost::remove_reference<decltype(std::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;

            forward_recurrence_iterator(const Recurrence& r, value_type f_n_minus_1, value_type f_n)
               : f_n_minus_1(f_n_minus_1), f_n(f_n), coef(r), k(0) {}

            forward_recurrence_iterator(const Recurrence& r, value_type f_n)
               : f_n(f_n), coef(r), k(0)
            {
               std::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<boost::math::policies::policy<> >();
               f_n_minus_1 = f_n * boost::math::tools::function_ratio_from_forwards_recurrence(detail::recurrence_offsetter<Recurrence>(r, -1), value_type(boost::math::tools::epsilon<value_type>() * 2), max_iter);
               boost::math::policies::check_series_iterations<value_type>("forward_recurrence_iterator<>::forward_recurrence_iterator", max_iter, boost::math::policies::policy<>());
            }

            forward_recurrence_iterator& operator++()
            {
               using std::swap;
               value_type a, b, c;
               boost::math::tie(a, b, c) = coef(k);
               value_type f_n_plus_1 = a * f_n_minus_1 / -c + b * f_n / -c;
               swap(f_n_minus_1, f_n);
               swap(f_n, f_n_plus_1);
               ++k;
               return *this;
            }

            forward_recurrence_iterator operator++(int)
            {
               forward_recurrence_iterator t(*this);
               ++(*this);
               return t;
            }

            value_type operator*() { return f_n; }

            value_type f_n_minus_1, f_n;
            Recurrence coef;
            int k;
         };

         template <class Recurrence>
         struct backward_recurrence_iterator
         {
            typedef typename boost::remove_reference<decltype(std::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;

            backward_recurrence_iterator(const Recurrence& r, value_type f_n_plus_1, value_type f_n)
               : f_n_plus_1(f_n_plus_1), f_n(f_n), coef(r), k(0) {}

            backward_recurrence_iterator(const Recurrence& r, value_type f_n)
               : f_n(f_n), coef(r), k(0)
            {
               std::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<boost::math::policies::policy<> >();
               f_n_plus_1 = f_n * boost::math::tools::function_ratio_from_backwards_recurrence(detail::recurrence_offsetter<Recurrence>(r, 1), value_type(boost::math::tools::epsilon<value_type>() * 2), max_iter);
               boost::math::policies::check_series_iterations<value_type>("backward_recurrence_iterator<>::backward_recurrence_iterator", max_iter, boost::math::policies::policy<>());
            }

            backward_recurrence_iterator& operator++()
            {
               using std::swap;
               value_type a, b, c;
               boost::math::tie(a, b, c) = coef(k);
               value_type f_n_minus_1 = c * f_n_plus_1 / -a + b * f_n / -a;
               swap(f_n_plus_1, f_n);
               swap(f_n, f_n_minus_1);
               --k;
               return *this;
            }

            backward_recurrence_iterator operator++(int)
            {
               backward_recurrence_iterator t(*this);
               ++(*this);
               return t;
            }

            value_type operator*() { return f_n; }

            value_type f_n_plus_1, f_n;
            Recurrence coef;
            int k;
         };

      }
   }
} // namespaces

#endif // BOOST_MATH_TOOLS_RECURRENCE_HPP_


/**
    @file bayes_classifier.cpp

    @author Terence Henriod

    Project 1: Bayesian Minimum Error Classification

    @brief Class implementations for the StrictGaussianClassifier defined in
           bayes_classifier.h.

    @version Original Code 1.00 (3/8/2014) - T. Henriod
*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   HEADER FILES / NAMESPACES
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

// Class Declaration
#include "strict_gaussian_classifier.h"

// Other Dependencies
#include <cassert>
#include <iostream>
#include <fstream>

#include "bayes_classifier.h"
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
================================================================================
                   CLASS FUNCTION IMPLEMENTATIONS
================================================================================
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   CONSTRUCTOR(S) / DESTRUCTOR
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/**
StrictGaussianClassifier

Description

@pre
-# The GameState object is given an appropriate identifier.

@post
-# A new, empty GameState will be initialized.

@code
@endcode
*/
StrictGaussianClassifier::StrictGaussianClassifier()
{
  // variables
  int ndx = 0;
  Eigen::Matrix2d temp_matrix;
    temp_matrix << 1, 0,
                   0, 1;

  // initialize all members
  class_name_ = "Give me a name!";
  mean_vector_ << 1, 1;
  set_covariance( temp_matrix );
  decision_threshold_ = 0.5;

  // no return - constructor
}

StrictGaussianClassifier::StrictGaussianClassifier(
    const StrictGaussianClassifier& other )
{
  // no return - copy constructor
}

StrictGaussianClassifier& StrictGaussianClassifier::operator=(
    const StrictGaussianClassifier& other )
{
  // return *this
  return *this;
}

StrictGaussianClassifier::~StrictGaussianClassifier()
{
  // currently nothing to destruct

  // no return - destructor
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   MUTATORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void StrictGaussianClassifier::clear()
{
  // no return - void
}

void StrictGaussianClassifier::set_mean( const Eigen::Vector2d& new_mean_vector )
{
  // set the appropriate mean vector
  mean_vector_ = new_mean_vector;

  // no return - void
}

void StrictGaussianClassifier::set_mean( const vector<DataItem>& data )
{
  // variables
  int i = 0;
  int num_data = 0;

  // reset the mean vector
  mean_vector_( 0 ) = 0;
  mean_vector_( 1 ) = 0;

  // sum the values of the features over all of the data
  for( i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      mean_vector_( 0 ) += data[i].feature_vector( 0 );
      mean_vector_( 1 ) += data[i].feature_vector( 1 );
      num_data++;
    }
  }

  // scale the data
  mean_vector_( 0 ) /= num_data;
  mean_vector_( 1 ) /= num_data;

  // no return - void
}

void StrictGaussianClassifier::set_covariance(
    const Eigen::Matrix2d& new_covariance_matrix )
{
  // set the appropriate covariance matrix
  covariance_matrix_ = new_covariance_matrix;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void StrictGaussianClassifier::set_covariance( const vector<DataItem>& data,
                                      const Eigen::Vector2d& mean )
{
  // variables
  int i = 0;
  int num_data = 0;

  // reset the covariance matrix
  covariance_matrix_ << 0, 0,
                        0, 0;

  // sum the values of the features over all of the data
  for( num_data = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sums
      covariance_matrix_( 0, 0 ) +=
          ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
          ( data[i].feature_vector( 0 ) - mean( 0 ) );
      covariance_matrix_( 1, 0 ) +=
          ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
          ( data[i].feature_vector( 0 ) - mean( 0 ) );
      covariance_matrix_( 1, 1 ) +=
          ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
          ( data[i].feature_vector( 1 ) - mean( 1 ) );
      num_data++;
    }
  }

  // set the covariance above the diagonal
  covariance_matrix_( 0, 1 ) = covariance_matrix_( 1, 0 );

  // scale the result
  covariance_matrix_ = ( 1.0 / ( (double) num_data - 1.0) ) *
                       covariance_matrix_;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void StrictGaussianClassifier::set_class_name( const string& new_name )
{
  // set the class name member
  class_name_ = new_name;
}

void StrictGaussianClassifier::set_decision_threshold(
    const double new_threshold )
{
  // set the class decision threshold member
  decision_threshold_ = new_threshold;
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   ACCESSORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

Eigen::Vector2d StrictGaussianClassifier::mean_vector() const
{
  // return the prior mean feature vector of the class
  return mean_vector_;
}

Eigen::Matrix2d StrictGaussianClassifier::covariance_matrix() const
{
  // return the covariance matrix of the class
  return covariance_matrix_;
}

Eigen::Matrix2d StrictGaussianClassifier::inverse_covariance_matrix() const
{
  // return the inverse of the covariance matrix of the class
  return inverse_covariance_matrix_;
}

double StrictGaussianClassifier::covariance_determinant() const
{
  // return the determinant of the covariance matrix of the class
  return covariance_determinant_;
}

string StrictGaussianClassifier::class_name() const
{
  // return the class name
  return class_name_;
}

double StrictGaussianClassifier::decision_threshold() const
{
  // return the decision threshold value
  return decision_threshold_;
}

void StrictGaussianClassifier::reportClassifierInfo()
{
  // variables
    // none

  // report the class name
  cout << "Classifier for class " << class_name_ << endl;

  // report the trained mean
  printf( "The training data mean is:\r\n" );
  cout << mean_vector_ << endl;

  // report the trained covariance
  printf( "The training data covariance is:\r\n" );
  cout << covariance_matrix_ << endl;

  // no return - void
}

bool StrictGaussianClassifier::objectIsInThisClass( Eigen::Vector2d& test_vector )
{
  // return the decision that the object is in this class
  return ( getGaussianProbability( test_vector ) > decision_threshold_ );
}

double StrictGaussianClassifier::getGaussianProbability(
    Eigen::Vector2d& test_vector )
{
  // variables
  double gaussian_probability_density = 0;
  double fractional_part = 1;
  double exponent_part = 0;
  Eigen::Vector2d test_mean_difference;
  Eigen::Vector2d intermediate_vector;

/*
    DON'T KNOW WHY, BUT THE SCALE FACTOR IS RUINING THINGS - IS DATA ALREADY
    NORMALIZED/SCALED SOMEHOW?

  // compute the normalizing/scale factor
  fractional_part = sqrt( 2 * PI );
  fractional_part = pow( fractional_part, DIMENSIONALITY );
  fractional_part *= sqrt( covariance_determinant_ );
  fractional_part = pow( fractional_part, -1 );
*/

  // compute the expontent part
  test_mean_difference = test_vector - mean_vector_;

  intermediate_vector = test_mean_difference.transpose() *
                        inverse_covariance_matrix_;
  exponent_part = -0.5 * ( intermediate_vector.dot( test_mean_difference ) );

  // compute the whole thing
  gaussian_probability_density = fractional_part * exp( exponent_part );

  // return the result
  return gaussian_probability_density;
}


//  (C) Copyright Eric Niebler 2005.
//  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 <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/moment.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::moment<2> > > acc1;

    acc1(2); //    4
    acc1(4); //   16
    acc1(5); // + 25
             // = 45 / 3 = 15

    BOOST_CHECK_CLOSE(15., accumulators::moment<2>(acc1), 1e-5);

    accumulator_set<int, stats<tag::moment<5> > > acc2;

    acc2(2); //     32
    acc2(3); //    243
    acc2(4); //   1024
    acc2(5); // + 3125
             // = 4424 / 4 = 1106

    BOOST_CHECK_CLOSE(1106., accumulators::moment<5>(acc2), 1e-5);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("moment test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <vector>
#include <iostream>
#include <unordered_map>
#include <Eigen/Eigen>
#include <fstream>
#include <cstdio>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include "orcvio/obj/ObjectState.h"

using namespace boost::filesystem;

namespace orcvio
{

Eigen::MatrixX3d transform_mean_keypoints_to_global(const Eigen::MatrixX3d& object_keypoints_mean_shape, const Eigen::Matrix4d& object_pose)
{

    // get the number of keypoints for this object class
    const int kps_num = object_keypoints_mean_shape.rows();

    Eigen::MatrixX3d object_keypoints_shape_global_frame;
    object_keypoints_shape_global_frame = Eigen::MatrixXd::Zero(kps_num, 3);

    Eigen::Matrix3d wRq = object_pose.block(0, 0, 3, 3);
    Eigen::Vector3d wPq = object_pose.block(0, 3, 3, 1);

    // std::cout << "wRq " << wRq << std::endl;
    // std::cout << "wPq " << wPq.transpose() << std::endl;

    for (int i = 0; i < kps_num; ++i)
    {
        // std::cout << "keypoint position in object frame " << object_keypoints_mean_shape.row(i) << std::endl;
        Eigen::Vector3d keypoint_global_frame = wRq * object_keypoints_mean_shape.row(i).transpose() + wPq;
        // std::cout << "keypoint position in global frame " << keypoint_global_frame.transpose() << std::endl;
        object_keypoints_shape_global_frame.row(i) = keypoint_global_frame.transpose();
    }

    return object_keypoints_shape_global_frame;

}

void save_object_state_to_file(const ObjectState & object_state, const std::vector<double>& timestamps,
    std::string filepath_format)
{
    boost::format boost_filepath_format(filepath_format);
    if (!boost::filesystem::is_directory(filepath_format))
        return;

    std::ofstream file((boost_filepath_format % object_state.object_id).str());

    // std::cout << "debug file " << file.is_open() << std::endl;

    if (file.is_open())
    {
        file << "object id:\n" << object_state.object_id << '\n';
        file << "object class:\n" << object_state.object_class << '\n';
        file << "wTq:\n" << object_state.object_pose << '\n';
        file << "keypoints in global frame:\n" << object_state.object_keypoints_shape_global_frame << '\n';
        file << "ellipsoid shape:\n" << object_state.ellipsoid_shape << '\n';
        file << "observation timestamps:\n";
        for (const auto& time: timestamps)
        {
            file << time << " ";
        }
    }
    // else
    //     std::cout << "cannot open file" << std::endl;

}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex toolbox - cot/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of cot  components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
///
#include <nt2/include/functions/cot.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/tan.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>
#include <complex>

NT2_TEST_CASE_TPL ( cot_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::cot;
  using nt2::tag::cot_;
  typedef std::complex<T> cT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<cot_(cT)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename nt2:: meta::as_complex<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(nt2::Inf<T>())), cT(nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(nt2::Minf<T>())), cT(nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(1, 1)),nt2::rec(nt2::tan(cT(1.0, 1.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(1, 10)),nt2::rec(nt2::tan(cT(1.0, 10.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(10, 1)),nt2::rec(nt2::tan(cT(10.0, 1.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(10, 10)),nt2::rec(nt2::tan(cT(10.0, 10.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(0, 1)),nt2::rec(nt2::tan(cT(0.0, 1.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(0, 10)),nt2::rec(nt2::tan(cT(0.0, 10.0))), 1);
  NT2_TEST_ULP_EQUAL(nt2::cot(cT(10, 0)),nt2::rec(nt2::tan(cT(10.0, 0.0))), 1);
 } // end of test for floating_


/*
Copyright 2014 Rogier van Dalen.

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.
*/

/** \file
Test max_semiring.hpp.
*/

#define BOOST_TEST_MODULE test_max_semiring
#include "utility/test/boost_unit_test.hpp"

#include "math/max_semiring.hpp"

#include <boost/mpl/assert.hpp>

#include "range/std/container.hpp"

#include "math/check/check_magma.hpp"
#include "math/check/check_hash.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_max_semiring)

template <class Type> void check_max_semiring_for() {
    typedef math::max_semiring <Type> semiring;

    {
        semiring five (5);
        semiring three (3);
        semiring zero (0);
        semiring zero2;

        BOOST_CHECK (zero == zero2);

        BOOST_CHECK_EQUAL ((five * three).value(), 15);
        BOOST_CHECK_EQUAL ((five + three), five);
        BOOST_CHECK_EQUAL (math::choose (five, three), five);

        BOOST_CHECK (three < five);
        BOOST_CHECK (zero < five);
        BOOST_CHECK (zero < three);
    }

    static_assert (math::has <math::callable::times (
        semiring, semiring)>::value, "");
    static_assert (math::has <math::callable::plus (
        semiring, semiring)>::value, "");
    static_assert (math::has <math::callable::choose (
        semiring, semiring)>::value, "");

    static_assert (math::is::associative <math::callable::times (
        semiring, semiring)>::value, "");
    static_assert (math::is::commutative <math::callable::times (
        semiring, semiring)>::value, "");
    static_assert (!math::is::idempotent <math::callable::times (
        semiring, semiring)>::value, "");

    static_assert (math::is::path_operation <math::callable::plus (
        semiring, semiring)>::value, "");
    static_assert (math::is::path_operation <math::callable::choose (
        semiring, semiring)>::value, "");

    std::vector <semiring> examples;
    examples.push_back (semiring (0));
    examples.push_back (semiring (3));
    examples.push_back (semiring (5));
    examples.push_back (semiring (17));

    BOOST_CHECK (semiring (3) == semiring (3));

    math::check_equal_on (examples);
    math::check_hash (examples);

    math::check_semiring <semiring, math::either> (
        math::times, math::plus, examples);
    math::check_semiring <semiring, math::either> (
        math::times, math::choose, examples);
}

BOOST_AUTO_TEST_CASE (test_max_semiring_complete) {
    check_max_semiring_for <int>();
    check_max_semiring_for <float>();
    check_max_semiring_for <double>();
}

// Test whether floating-point numbers and integers are treated correctly when
// they should behave differently.
BOOST_AUTO_TEST_CASE (test_max_semiring_float) {
    // non_member, divide, and invert available for floating-point numbers.
    static_assert (math::has <
        math::callable::non_member <math::max_semiring <double>>()>::value, "");
    static_assert (math::has <
        math::callable::invert <math::callable::times> (
            math::max_semiring <double>)>::value, "");
    static_assert (math::has <math::callable::divide<> (
            math::max_semiring <double>, math::max_semiring <double>)>::value,
        "");

    // times and divide are approximate for floating-point numbers; plus is not.
    static_assert (!math::is::approximate <math::callable::plus (
        math::max_semiring <double>, math::max_semiring <double>)>::value, "");
    static_assert (math::is::approximate <math::callable::times (
        math::max_semiring <double>, math::max_semiring <double>)>::value, "");
    static_assert (math::is::approximate <math::callable::divide<> (
        math::max_semiring <double>, math::max_semiring <double>)>::value, "");

    // But not for integers.
    static_assert (!math::has <
        math::callable::non_member <math::max_semiring <int>>()>::value, "");
    static_assert (!math::has <math::callable::invert <math::callable::times> (
            math::max_semiring <int>)>::value, "");
    static_assert (!math::has <math::callable::divide<> (
            math::max_semiring <int>, math::max_semiring <int>)>::value, "");

    static_assert (!math::is::approximate <math::callable::plus (
        math::max_semiring <int>, math::max_semiring <int>)>::value, "");
    static_assert (!math::is::approximate <math::callable::times (
        math::max_semiring <int>, math::max_semiring <int>)>::value, "");
    static_assert (!math::is::approximate <math::callable::divide<> (
        math::max_semiring <int>, math::max_semiring <int>)>::value, "");
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Simulate: Particles from Form+Code in Art, Design, and Architecture
 * implemented in C++ by Patrick Tierney (patrick.l.tierney@gmail.com || http://ptierney.com)
 *
 * Requires Cinder 0.8.2 available at http://libcinder.org
 *
 * Project files are located at https://github.com/hlp/form-and-code
 *
 * For more information about Form+Code visit http://formandcode.com
 */

#include <boost/date_time.hpp>

#include "cinder/app/AppBasic.h"
#include "cinder/Rand.h"
#include "cinder/CinderMath.h"

class Particle;

class Simulate_Particles : public ci::app::AppBasic {
public:
    void prepareSettings(Settings* settings);
    void setup();
    void draw();

private:
    std::vector<Particle> particles;
    bool saving;
};

class Particle {
public:
    Particle(ci::Vec2f l) {
        counter = 0;

        float randmin = -M_PI / 2.0f;
        float randmax = 0;

        float r = ci::Rand::randFloat(0, M_PI * 2.0f);
        float x = ci::math<float>::cos(r);
        float y = ci::math<float>::sin(r);
        acc = ci::Vec2f(x / 250.0f, y / 250.0f);

        float q = ci::Rand::randFloat(0, 1);
        r = ci::Rand::randFloat(randmin, randmax);
        x = ci::math<float>::cos(r) * q;
        y = ci::math<float>::sin(r) * q;
        vel = ci::Vec2f(x, y);
        loc = l;

        counter = 0;
        hist.resize(1000);
    }

    void update() {
        vel += acc;
        loc += vel;
        // save location every 10 frames
        if (ci::app::getElapsedFrames() % 10 == 0) {
            hist[counter] = (loc);
            counter++;
        }
    }

    void drawArrowHead(ci::Vec2f v, ci::Vec2f loc, float scale) {
        ci::gl::pushMatrices();
        float arrowsize = 4;
        // Translate to location to render vector
        ci::gl::translate(ci::Vec2f(loc.x, loc.y));

        // Rotate to the vector heading
        ci::Quatf q(0.0f, 0.0f, ci::math<float>::atan2(v.normalized().y, v.normalized().x));
        ci::gl::rotate(q);

        // Calculate length of vector & scale it to be bigger or smaller if necessary
        float len = v.length()*scale;
        arrowsize = ci::lmap<float>(len, 0, 10, 0, 1) * arrowsize;

        // Draw point
        glColor4f(0.0f, 0.0f, 0.0f, 100.0f/255.0f);
        ci::gl::drawLine(ci::Vec2f(0,0),ci::Vec2f(len-arrowsize,0));
        glBegin(GL_TRIANGLES);
        glVertex2f(len,0);
        glVertex2f(len-arrowsize,+arrowsize/2);
        glVertex2f(len-arrowsize,-arrowsize/2);
        glEnd();

        ci::gl::popMatrices();
    }

    void draw() {
        float c = 100.0f/255.0f;
        glColor4f(c, c, c, 50.0f/255.0f);

        drawArrowHead(vel,loc,10);

        // draw history path
        glColor4f(0.0f, 0.0f, 0.0f, 100.0f/255.0f);
        glBegin(GL_LINE_STRIP);
        for (int i = 0; i < counter; i++) {
            glVertex2f(hist[i].x, hist[i].y);
        }

        if (!hist.empty()) glVertex2f(loc.x, loc.y);

        glEnd();
    }

private:
    ci::Vec2f loc;
    ci::Vec2f vel;
    ci::Vec2f acc;

    std::vector<ci::Vec2f> hist;
    int counter;
};

void Simulate_Particles::prepareSettings(Settings* settings) {
    settings->setWindowSize(1024, 768);
}

void Simulate_Particles::setup() {
    glEnable(GL_LINE_SMOOTH);
    ci::gl::enableAlphaBlending();

    for (int i = 0; i < 1000; i++) {
        particles.push_back(Particle(ci::Vec2f(100, getWindowHeight()-100)));
    }
}

void Simulate_Particles::draw() {
    ci::gl::setMatricesWindow(getWindowSize());
    ci::gl::clear(ci::Color::white());

    for (std::vector<Particle>::iterator it = particles.begin();
        it != particles.end(); ++it) {
        it->update();
        it->draw();
    }
}

CINDER_APP_BASIC(Simulate_Particles, ci::app::RendererGl)


#ifndef _POW_HPP
#define _POW_HPP

#include "discrete_sequence.hpp"

#include <boost/hana/integral_constant.hpp>
#include <cstdlib>

template<long long N, typename T>
auto power(T x) {
    if constexpr (N == 0) {
        return T{1};
    } else {
        return x * power<N-1>(x);
    }
};

template <long long N, typename UnderlyingSequence>
struct pow_expr
{
    constexpr explicit pow_expr(UnderlyingSequence const& seq) : seq{seq} {};

    constexpr unsigned operator()(size_t index) const {
        return power<N>(seq(index));
    }

    UnderlyingSequence const& seq;
};

template <typename Sequence, long long N>
constexpr auto operator^(Sequence const& seq, boost::hana::integral_constant<long long, N>)
{
    return pow_expr<N, Sequence>{seq};
}

#endif //_POW_HPP


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <mpi.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;
static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt);

void calc_minimal(GF2X& minimal, int maxdegree, w128_t outseq[], int bitpos)
{
    uint64_t mask;
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    int idx;
    if (bitpos >= 64) {
	idx = 1;
	mask = UINT64_C(1) << (bitpos - 64);
    } else {
	idx = 0;
	mask = UINT64_C(1) << bitpos;
    }
    for (int i = 0; i < 2 * maxdegree; i++) {
	if (outseq[i].u[idx] & mask) {
	    seq[i] = 1;
	} else {
	    seq[i] = 0;
	}
    }
    MinPolySeq(minimal, seq, maxdegree);
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
    lcm /= gcd;
}

void get_lcm(int rank, int num_process, GF2X& lcmpoly, dSFMText& dsfmt) {
    int maxdegree = dsfmt.get_mamaxdegree();
    dsfmt.seeding(1234);
    get_lcm_sub(lcmpoly, dsfmt);
    int unit = maxdegree / num_process;
    int start = rank * num_process;
    for(int i = start; i < start + unit; i++) {
	dsfmt.init_basis(i);
	get_lcm_sub(lcmpoly, dsfmt);
    }
}

static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X tmp;
    int mamaxdegree = dsfmt.get_mamaxdegree();
    w128_t out_seq[2 * mamaxdegree];
    int i, bitpos;

    for (int i = 0; i < 2 * mamaxdegree; i++) {
	out_seq[i] = dsfmt.next();
    }

    GF2X minimal;
    for (bitpos = 0; bitpos < 128; bitpos++) {
	calc_minimal(minimal, mamaxdegree, out_seq, bitpos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
    }
}

#if defined(IRRE_CHECK)
static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}
#endif

int main(int argc, char *argv[]) {
    int rank;
    int num_process;
    int MPI_Status status;
    MPI_Init(&argc, &argv);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &num_process);
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	MPI_Finalize();
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
    stringstream ss;
    ss << "lcm." << dec << mexp << "." << rank << ".txt";
    string fname;
    ss >> fname;
    //ofstream fout("/tmp/results.txt", ios::trunc );
    ofstream fout(fname.c_str());

    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(rank, num_process, characteristic, dsfmt);
    GF2X work;
    work = characteristic;
#if defined(IRRE_CHECK)
    if (!has_large_irreducible(characteristic, mexp)) {
        fout << "error? does not have large irreducible" << endl;
	MPI_Finalize();
        return -1;
    }
#endif
    string x;
    polytostring(x, work);
    fout << "#" << dec << mexp;
    fout << "," << dec << pos1;
    fout << "," << dec << sl1;
    fout << "," << hex << mask[0];
    fout << "," << hex << mask[1];
    fout << dec << endl;
    fout << x << endl;
    fout << dec << flush;
    MPI_Finalize();
}


#include "../DynAutoDiff/DynAutoDiff.hpp"
#include <algorithm>
#include <boost/test/tools/old/interface.hpp>
#include <eigen3/Eigen/Core>
#include <iostream>
#include <vector>

#define BOOST_TEST_MODULE ArithMetic_Test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#define TL 1e-10

using namespace Eigen;
using namespace std;
using namespace DynAutoDiff;

BOOST_AUTO_TEST_SUITE(test)
BOOST_AUTO_TEST_CASE(multi_branch_test) {
    auto x1=psca(2.0),x2=psca(3.0),x3=psca(4.0),x4=psca(5.0);
	auto y1=x1*x2+x3;
	auto y2=y1*x4+y1;

	GraphManager<> m1(y2);
	m1.run();
	BOOST_CHECK_CLOSE(y2->v(), 60.0, 1e-5);
	BOOST_CHECK_CLOSE(x1->g(), 18.0, 1e-5);
}
BOOST_AUTO_TEST_SUITE_END()


#include<CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include<CGAL/create_straight_skeleton_2.h>
#include<CGAL/draw_straight_skeleton_2.h>
#include "print.h"
#include<CGAL/Polygon_2.h>

#include <boost/shared_ptr.hpp>

#include <cassert>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;

typedef K::Point_2                                                  Point;
typedef CGAL::Polygon_2<K>                                          Polygon_2;
typedef CGAL::Straight_skeleton_2<K>                                Ss;

typedef boost::shared_ptr<Ss>                                       SsPtr;

int main()
{
  Polygon_2 poly;

  poly.push_back(Point(     0,     0));
  poly.push_back(Point(  2000,  8000));
  poly.push_back(Point( 10000, 10000));
  poly.push_back(Point(  2000, 12000));
  poly.push_back(Point(     0, 20000));
  poly.push_back(Point( -2000, 12000));
  poly.push_back(Point(-10000, 10000));
  poly.push_back(Point( -2000,  8000));

  assert(poly.is_simple());
  assert(poly.is_counterclockwise_oriented());

  SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());

  print_straight_skeleton(*iss);
  draw(*iss);

  assert(iss->size_of_vertices() == 9);
  assert(iss->size_of_halfedges() == 32);
  assert(iss->size_of_faces() == 8);
  assert(iss->is_valid());

  return EXIT_SUCCESS;
}


#ifndef SOAR_H_
#define SOAR_H_

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

class Soar {
public:
  Soar(const Eigen::Ref<const Eigen::MatrixXd> &matA,
       const Eigen::Ref<const Eigen::MatrixXd> &matB);

  Eigen::MatrixXd compute(int n);

private:
  const int ndim_;
  Eigen::MatrixXd matA_, matB_;
  Eigen::VectorXd u_;

  const double tol_ = 1.0e-10;
};

#endif


#include <adept.h>
#include <benchmark/benchmark.h>
#include <ceres/autodiff_cost_function.h>
#include <game/vsr/cga_op.h>
#include <glog/logging.h>
#include <hep/ga.hpp>
#include <vahlen/vahlen.h>

using namespace vsr::cga;

adept::Stack g_stack;

double g_vector[3] = {1.0, 2.0, 3.0};
double g_bivector[3] = {1.0, 2.0, 3.0};
double g_motor[8] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double g_point[5] = {1.0, 2.0, 3.0, 1.0, 7.0};
double g_point_spin_motor[5];
double g_vec_ip_biv[3] = {0.0, 0.0, 0.0};

template <typename T>
void InnerProductVectorBivector(const T *vec, const T *biv, T *res) {
  Vector<T> vector(vec);
  Bivector<T> bivector(biv);
  Vector<T> result = vector <= bivector;
  for (int i = 0; i < 3; ++i)
    res[i] = result[i];
}

struct InnerProductVectorBivectorFunctor {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector(vec, biv, res);
    return true;
  }
};

template <typename T>
void InnerProductVectorBivector2(const T *a, const T *b, T *res) {
  res[0] = -a[2] * b[1] - a[1] * b[0];
  res[1] = -a[2] * b[2] + a[0] * b[0];
  res[2] = a[1] * b[2] + a[0] * b[1];
}

struct InnerProductVectorBivectorFunctor2 {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector2(vec, biv, res);
    return true;
  }
};

template <typename T>
void GaalopCalculateMotorSpinPoint(const T m1, const T m2, const T m3,
                                   const T m4, const T m5, const T m6,
                                   const T m7, const T m8, const T p1,
                                   const T p2, const T p3, const T p4,
                                   const T p5, T *q) {

  q[0] = ((-(2.0 * m1 * m5)) - 2.0 * m2 * m6 - 2.0 * m3 * m7 - 2.0 * m4 * m8) *
             p4 +
         (2.0 * m1 * m3 + 2.0 * m2 * m4) * p3 +
         (2.0 * m1 * m2 - 2.0 * m3 * m4) * p2 +
         (m1 * m1 - m2 * m2 - m3 * m3 + m4 * m4) * p1; // e1
  q[1] = (2.0 * m2 * m5 - 2.0 * m1 * m6 - 2.0 * m4 * m7 + 2.0 * m3 * m8) * p4 +
         (2.0 * m1 * m4 - 2.0 * m2 * m3) * p3 +
         ((m1 * m1 - m2 * m2 + m3 * m3) - m4 * m4) * p2 +
         ((-(2.0 * m1 * m2)) - 2.0 * m3 * m4) * p1; // e2
  q[2] =
      ((2.0 * m3 * m5 + 2.0 * m4 * m6) - 2.0 * m1 * m7 - 2.0 * m2 * m8) * p4 +
      ((m1 * m1 + m2 * m2) - m3 * m3 - m4 * m4) * p3 +
      ((-(2.0 * m2 * m3)) - 2.0 * m1 * m4) * p2 +
      (2.0 * m2 * m4 - 2.0 * m1 * m3) * p1;            // e3
  q[3] = (m4 * m4 + m3 * m3 + m2 * m2 + m1 * m1) * p4; // e0
  q[4] =
      (m1 * m1 + m2 * m2 + m3 * m3 + m4 * m4) * p5 +
      (2.0 * m5 * m5 + 2.0 * m6 * m6 + 2.0 * m7 * m7 + 2.0 * m8 * m8) * p4 +
      ((-(2.0 * m3 * m5)) - 2.0 * m4 * m6 - 2.0 * m1 * m7 - 2.0 * m2 * m8) *
          p3 +
      ((-(2.0 * m2 * m5)) - 2.0 * m1 * m6 + 2.0 * m4 * m7 + 2.0 * m3 * m8) *
          p2 +
      (((-(2.0 * m1 * m5)) + 2.0 * m2 * m6 + 2.0 * m3 * m7) - 2.0 * m4 * m8) *
          p1; // einf
}

template <typename T>
void GaalopMotorSpinPoint(const T *mot, const T *pnt, T *res) {
  GaalopCalculateMotorSpinPoint(mot[0], mot[1], mot[2], mot[3], mot[4], mot[5],
                                mot[6], mot[7], pnt[0], pnt[1], pnt[2], pnt[3],
                                pnt[4], res);
}

struct GaalopMotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    GaalopMotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T>
void VahlenMotorSpinPoint(const T *mot, const T *pnt, T *res) {
  using Mat = vahlen::Matrix<T>;
  Mat motor = vahlen::Motor<T>(mot);
  Mat point = vahlen::Point<T>(pnt);
  Mat result = motor * point * vahlen::Reverse(motor);
}

struct VahlenMotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    VahlenMotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T> void MotorSpinPoint(const T *mot, const T *pnt, T *res) {
  Motor<T> motor(mot);
  Point<T> point(pnt);
  Point<T> result = point.spin(motor);
  for (int i = 0; i < 5; ++i)
    res[i] = result[i];
}

struct MotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    MotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T> void RotorSpinPoint(const T *rot, const T *pnt, T *res) {
  Rotor<T> rotor(rot);
  Vector<T> point(pnt);
  Vector<T> result = point.spin(rotor);
  for (int i = 0; i < 3; ++i)
    res[i] = result[i];
}

struct RotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *rot, const T *pnt, T *res) const {
    RotorSpinPoint(rot, pnt, res);
    return true;
  }
};

static void BM_InnerProductVectorBivector(benchmark::State &state) {
  while (state.KeepRunning()) {
    InnerProductVectorBivector(g_vector, g_bivector, g_vec_ip_biv);
  }
}

static void BM_InnerProductVectorBivector2(benchmark::State &state) {
  while (state.KeepRunning()) {
    InnerProductVectorBivector2(g_vector, g_bivector, g_vec_ip_biv);
  }
}

static void BM_AdeptJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptJacobianForward2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector2(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptJacobianReverse(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_AdeptJacobianReverse2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector2(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_CeresJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    double vector[3] = {1.0, 2.0, 3.0};
    double bivector[3] = {1.0, 2.0, 3.0};
    double vec_ip_biv[3] = {0.0, 0.0, 0.0};
    const double *parameters[2] = {&vector[0], &bivector[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<InnerProductVectorBivectorFunctor, 3, 3, 3>(
        new InnerProductVectorBivectorFunctor())
        .Evaluate(parameters, &vec_ip_biv[0], jacobians);
  }
}

static void BM_CeresJacobian2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    double vector[3] = {1.0, 2.0, 3.0};
    double bivector[3] = {1.0, 2.0, 3.0};
    double vec_ip_biv[3] = {0.0, 0.0, 0.0};
    const double *parameters[2] = {&vector[0], &bivector[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<InnerProductVectorBivectorFunctor2, 3, 3, 3>(
        new InnerProductVectorBivectorFunctor2())
        .Evaluate(parameters, &vec_ip_biv[0], jacobians);
  }
}

static void BM_MotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    MotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_VahlenMotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    VahlenMotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_GaalopMotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    GaalopMotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_AdeptMotorSpinPointJacobianReverse(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    adept::adouble motor[8];
    adept::set_values(motor, 8, g_motor);
    adept::adouble point[5];
    adept::set_values(point, 5, g_point);
    g_stack.new_recording();
    adept::adouble res[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    MotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 8);
    g_stack.dependent(res, 5);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_AdeptMotorSpinPointJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    adept::adouble motor[8];
    adept::set_values(motor, 8, g_motor);
    adept::adouble point[5];
    adept::set_values(point, 5, g_point);
    g_stack.new_recording();
    adept::adouble res[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    MotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 8);
    g_stack.dependent(res, 5);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptRotorSpinPointJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[3 * 4];
    adept::adouble motor[4];
    adept::set_values(motor, 4, g_motor);
    adept::adouble point[3];
    adept::set_values(point, 3, g_point);
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    RotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 4);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_CeresMotorSpinPointJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    const double *parameters[2] = {&g_motor[0], &g_point[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<MotorSpinPointFunctor, 5, 8, 5>(
        new MotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_CeresGaalopMotorSpinPointJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    const double *parameters[2] = {&g_motor[0], &g_point[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<GaalopMotorSpinPointFunctor, 5, 8, 5>(
        new GaalopMotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_CeresRotorSpinPointJacobian(benchmark::State &state) {
  double jac[3 * 4];
  const double *parameters[2] = {&g_motor[0], &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<RotorSpinPointFunctor, 3, 4, 3>(
        new RotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

template <typename T> using Matrix4 = Eigen::Matrix<T, 4, 4>;

template <typename T> inline static Matrix4<T> s() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(1), T(0),
      T(0), T(0), T(0), T(1);
  return m;
}

template <typename T> inline static Matrix4<T> e1() {
  Matrix4<T> m;
  m << T(0), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0),
      T(1), T(0), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e2() {
  Matrix4<T> m;
  m << T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(1), T(0), T(0), T(0),
      T(0), T(-1), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e3() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(0),
      T(0), T(0), T(0), T(-1);
  return m;
}

struct DiffRotorMatrixFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Matrix4<T> rotor =
        cos(T(0.5) * th[0]) * s<T>() - sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> rotor_inv =
        cos(T(0.5) * th[0]) * s<T>() + sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> vec_a = a[0] * e1<T>() + a[1] * e2<T>() + a[2] * e3<T>();
    Matrix4<T> vec_b = rotor * vec_a * rotor_inv;
    b[0] = vec_b(0, 3); // e1
    b[1] = vec_b(0, 2); // e2
    b[2] = vec_b(0, 0); // e3
    return true;
  }
};

static void BM_CeresRotorMatrixJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorMatrixFunctor, 3, 1, 3>(
        new DiffRotorMatrixFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

struct DiffRotorVersorFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Rotor<T> rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector<T> vec_a{a[0], a[1], a[2]};
    Vector<T> vec_b = vec_a.spin(rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = vec_b[i];
    return true;
  }
};

static void BM_CeresRotorVersorJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorVersorFunctor, 3, 1, 3>(
        new DiffRotorVersorFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

struct DiffRotorHepGAFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    using Algebra = hep::algebra<T, 3, 0>;
    using Rotor = hep::multi_vector<Algebra, hep::list<0, 3, 5, 6>>;
    using Vector = hep::multi_vector<Algebra, hep::list<1, 2, 4>>;
    Rotor rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector pnt_a{a[0], a[1], a[2]};
    Vector pnt_b = hep::grade<1>(rotor * pnt_a * ~rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = pnt_b[i];
    return true;
  }
};

static void BM_CeresRotorHepGAJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorHepGAFunctor, 3, 1, 3>(
        new DiffRotorHepGAFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_AdeptRotorHepGAJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorHepGAJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorVersorJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorVersorJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorMatrixJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorMatrixFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorMatrixJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorMatrixFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

struct DiffRotorHandFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    T st = sin(th[0] / 2.0);
    T ct = cos(th[0] / 2.0);
    T stst = st * st;
    T ctct = ct * ct;
    T ctst = ct * st;
    b[0] = (-(a[0] * stst)) - 2.0 * a[1] * ctst + a[0] * ctct; // e1
    b[1] = (-(a[1] * stst)) + 2.0 * a[0] * ctst + a[1] * ctct; // e2
    b[2] = a[2] * stst + a[2] * ctct;                          // e3
    return true;
  }
};

struct DiffRotorGaalopFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    b[0] = (-(a[0] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) -
           2.0 * a[1] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[0] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e1
    b[1] = (-(a[1] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) +
           2.0 * a[0] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[1] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e2
    b[2] = a[2] * sin(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[2] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e3
    return true;
  }
};

static void BM_AdeptRotorGaalopJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_RotorMatrix(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
  }
}
static void BM_RotorHand(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
  }
}
static void BM_RotorGaalop(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_RotorVersor(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_RotorHepGA(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_AdeptRotorGaalopJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorHandJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorHandJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
    g_stack.set_max_jacobian_threads(3);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    // g_stack.jacobian_forward_openmp(jac, true);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_CeresRotorHandJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorHandFunctor, 3, 1, 3>(
        new DiffRotorHandFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}
static void BM_CeresRotorGaalopJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorGaalopFunctor, 3, 1, 3>(
        new DiffRotorGaalopFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

BENCHMARK(BM_InnerProductVectorBivector);
BENCHMARK(BM_AdeptJacobianForward);
BENCHMARK(BM_AdeptJacobianReverse);
BENCHMARK(BM_CeresJacobian);
BENCHMARK(BM_InnerProductVectorBivector2);
BENCHMARK(BM_AdeptJacobianForward2);
BENCHMARK(BM_AdeptJacobianReverse2);
BENCHMARK(BM_CeresJacobian2);
BENCHMARK(BM_MotorSpinPoint);
BENCHMARK(BM_VahlenMotorSpinPoint);
BENCHMARK(BM_GaalopMotorSpinPoint);
BENCHMARK(BM_AdeptMotorSpinPointJacobianForward);
BENCHMARK(BM_AdeptMotorSpinPointJacobianReverse);
BENCHMARK(BM_CeresGaalopMotorSpinPointJacobian);
BENCHMARK(BM_CeresMotorSpinPointJacobian);
BENCHMARK(BM_CeresRotorSpinPointJacobian);
BENCHMARK(BM_AdeptRotorSpinPointJacobianForward);

// AMDO paper
BENCHMARK(BM_CeresRotorMatrixJacobian);
BENCHMARK(BM_CeresRotorVersorJacobian);
BENCHMARK(BM_CeresRotorHepGAJacobian);
BENCHMARK(BM_CeresRotorGaalopJacobian);
BENCHMARK(BM_CeresRotorHandJacobian);
BENCHMARK(BM_AdeptRotorMatrixJacobianForward);
BENCHMARK(BM_AdeptRotorMatrixJacobianReverse);
BENCHMARK(BM_AdeptRotorVersorJacobianForward);
BENCHMARK(BM_AdeptRotorVersorJacobianReverse);
BENCHMARK(BM_AdeptRotorHepGAJacobianForward);
BENCHMARK(BM_AdeptRotorHepGAJacobianReverse);
BENCHMARK(BM_AdeptRotorGaalopJacobianReverse);
BENCHMARK(BM_AdeptRotorGaalopJacobianForward);
BENCHMARK(BM_AdeptRotorHandJacobianReverse);
BENCHMARK(BM_AdeptRotorHandJacobianForward);

BENCHMARK(BM_RotorHand);
BENCHMARK(BM_RotorGaalop);
BENCHMARK(BM_RotorVersor);
BENCHMARK(BM_RotorMatrix);
BENCHMARK(BM_RotorHepGA);

BENCHMARK_MAIN()


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization.h>
#include <dlib/svm.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.oca");

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

    class test_oca : public tester
    {

    public:
        test_oca (
        ) :
            tester ("test_oca",
                    "Runs tests on the oca component.")
        {
        }

        void perform_test(
        )
        {
            print_spinner();

            typedef matrix<double,0,1> w_type;
            w_type w;

            decision_function<linear_kernel<w_type> > df;
            svm_c_linear_trainer<linear_kernel<w_type> > trainer;
            trainer.set_c_class1(2);
            trainer.set_c_class1(3);
            trainer.set_learns_nonnegative_weights(true);
            trainer.set_epsilon(1e-12);

            std::vector<w_type> x;
            w_type temp(2);
            temp = -1, 1;
            x.push_back(temp);
            temp = 1, -1;
            x.push_back(temp);

            std::vector<double> y;
            y.push_back(+1);
            y.push_back(-1);

            w_type true_w(3);

            oca solver;

            // test the version without a non-negativity constraint on w.
            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 0);
            dlog << LINFO << trans(w);
            true_w = -0.5, 0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            w_type prior = true_w;
            solver(make_oca_problem_c_svm<w_type>(20.0, 30.0, mat(x), mat(y), false, 1e-12, 40), w, prior);
            dlog << LINFO << trans(w);
            true_w = -0.5, 0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            prior = 0,0,0;
            solver(make_oca_problem_c_svm<w_type>(20.0, 30.0, mat(x), mat(y), false, 1e-12, 40), w, prior);
            dlog << LINFO << trans(w);
            true_w = -0.5, 0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            prior = -1,1,0;
            solver(make_oca_problem_c_svm<w_type>(20.0, 30.0, mat(x), mat(y), false, 1e-12, 40), w, prior);
            dlog << LINFO << trans(w);
            true_w = -1.0, 1.0, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            prior = -0.2,0.2,0;
            solver(make_oca_problem_c_svm<w_type>(20.0, 30.0, mat(x), mat(y), false, 1e-12, 40), w, prior);
            dlog << LINFO << trans(w);
            true_w = -0.5, 0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            prior = -10.2,-1,0;
            solver(make_oca_problem_c_svm<w_type>(20.0, 30.0, mat(x), mat(y), false, 1e-12, 40), w, prior);
            dlog << LINFO << trans(w);
            true_w = -10.2, -1.0, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            // test the version with a non-negativity constraint on w.
            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 9999);
            dlog << LINFO << trans(w);
            true_w = 0, 1, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            df = trainer.train(x,y);
            w = join_cols(df.basis_vectors(0), uniform_matrix<double>(1,1,-df.b));
            true_w = 0, 1, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            // test the version with a non-negativity constraint on w.
            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 2);
            dlog << LINFO << trans(w);
            true_w = 0, 1, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            // test the version with a non-negativity constraint on w.
            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 1);
            dlog << LINFO << trans(w);
            true_w = 0, 1, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            // switching the labels should change which w weight goes negative.
            y.clear();
            y.push_back(-1);
            y.push_back(+1);

            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 0);
            dlog << LINFO << trans(w);
            true_w = 0.5, -0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 1);
            dlog << LINFO << trans(w);
            true_w = 0.5, -0.5, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 2);
            dlog << LINFO << trans(w);
            true_w = 1, 0, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            print_spinner();

            solver(make_oca_problem_c_svm<w_type>(2.0, 3.0, mat(x), mat(y), false, 1e-12, 40), w, 5);
            dlog << LINFO << trans(w);
            true_w = 1, 0, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            df = trainer.train(x,y);
            w = join_cols(df.basis_vectors(0), uniform_matrix<double>(1,1,-df.b));
            true_w = 1, 0, 0;
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            x.clear();
            y.clear();
            temp = -2, 2;
            x.push_back(temp);
            temp = 0, -0;
            x.push_back(temp);

            y.push_back(+1);
            y.push_back(-1);

            trainer.set_c(10);
            df = trainer.train(x,y);
            w = join_cols(df.basis_vectors(0), uniform_matrix<double>(1,1,-df.b));
            true_w = 0, 1, -1;
            dlog << LINFO << "w: " << trans(w);
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

            x.clear();
            y.clear();
            temp = -2, 2;
            x.push_back(temp);
            temp = 0, -0;
            x.push_back(temp);

            y.push_back(-1);
            y.push_back(+1);

            trainer.set_c(10);
            df = trainer.train(x,y);
            w = join_cols(df.basis_vectors(0), uniform_matrix<double>(1,1,-df.b));
            true_w = 1, 0, 1;
            dlog << LINFO << "w: " << trans(w);
            dlog << LINFO << "error: "<< max(abs(w-true_w));
            DLIB_TEST(max(abs(w-true_w)) < 1e-10);

        }

    } a;

}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the multilayer perceptron
    from the dlib C++ Library.

    This example creates a simple set of data to train on and shows
    you how to train a mlp object on that data.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled 1 and all other points are labeled
    as 0.

*/

#include <iostream>
#include <dlib/mlp.h>

using namespace std;
using namespace dlib;

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_mlp_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    // The mlp takes column vectors as input and gives column vectors as output.  The dlib::matrix
    // object is used to represent the column vectors. So the first thing we do here is declare
    // a convenient typedef for the matrix object we will be using.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.   (Note that if you wanted
    // more than 2 features in this vector you can simply change the 2 to something else)
    typedef matrix<double, 2, 1> sample_type;

    // make an instance of a sample matrix so we can use it below
    sample_type sample;

    // Create a multi-layer perceptron network.   This network has 2 nodes on the input layer
    // (which means it takes column vectors of length 2 as input) and 5 nodes in the first
    // hidden layer.  Note that the other 4 variables in the mlp's constructor are left at
    // their default values.
    mlp::kernel_1a_c net(2,5);

    // Now let's put some data into our sample and train on it.  We do this
    // by looping over 41*41 points and labeling them according to their
    // distance from the origin.
    for (int i = 0; i < 1000; ++i)
    {
        for (int r = -20; r <= 20; ++r)
        {
            for (int c = -20; c <= 20; ++c)
            {
                sample(0) = r;
                sample(1) = c;

                // if this point is less than 10 from the origin
                if (sqrt((double)r*r + c*c) <= 10)
                    net.train(sample,1);
                else
                    net.train(sample,0);
            }
        }
    }

    // Now we have trained our mlp.  Let's see how well it did.
    // Note that if you run this program multiple times you will get different results. This
    // is because the mlp network is randomly initialized.

    // each of these statements prints out the output of the network given a particular sample.

    sample(0) = 3.123;
    sample(1) = 4;
    cout << "This sample should be close to 1 and it is classified as a " << net(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This sample should be close to 0 and it is classified as a " << net(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This sample should be close to 0 and it is classified as a " << net(sample) << endl;
}


// This file is part of the dune-xt project:
//   https://github.com/dune-community/dune-xt
// Copyright 2009-2018 dune-xt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   René Fritze    (2018 - 2019)
//   Tobias Leibner (2018)

#ifndef DUNE_XT_DATA_COORDINATES_HH
#define DUNE_XT_DATA_COORDINATES_HH

#include <boost/geometry.hpp>

#include <dune/common/fvector.hh>

namespace Dune::XT::Data {

/** Converts from (x, y, z) to (theta, phi) on the unit sphere s.t.
 * (x, y, z) = (sin(theta) cos(phi), sin(theta) sin(phi), cos(theta))
 * with 0 \leq \theta \leq \pi and 0 \leq \varphi < 2\pi. **/
template <class DomainFieldType>
class CoordinateConverter
{
  using BoostCartesianCoordType =
      typename boost::geometry::model::point<DomainFieldType, 3, typename boost::geometry::cs::cartesian>;
  using BoostSphericalCoordType = typename boost::geometry::model::
      point<DomainFieldType, 2, typename boost::geometry::cs::spherical<boost::geometry::radian>>;

public:
  using CartesianCoordType = FieldVector<DomainFieldType, 3>;
  using SphericalCoordType = FieldVector<DomainFieldType, 2>;

  static SphericalCoordType to_spherical(const CartesianCoordType& x)
  {
    BoostCartesianCoordType x_boost(x[0], x[1], x[2]);
    BoostSphericalCoordType x_spherical_boost;
    boost::geometry::transform(x_boost, x_spherical_boost);
    return SphericalCoordType{boost::geometry::get<1>(x_spherical_boost), boost::geometry::get<0>(x_spherical_boost)};
  }

  static CartesianCoordType to_cartesian(const SphericalCoordType& x_spherical, bool first_is_cosine = false)
  {
    // if first_is_cosine, the first coordinate is not theta but rather cos(theta)
    if (first_is_cosine) {
      const auto& mu = x_spherical[0];
      const auto& phi = x_spherical[1];
      return CartesianCoordType{
          std::sqrt(1 - std::pow(mu, 2)) * std::cos(phi), std::sqrt(1 - std::pow(mu, 2)) * std::sin(phi), mu};
    }
    BoostSphericalCoordType x_spherical_boost(x_spherical[1], x_spherical[0]);
    BoostCartesianCoordType x_boost;
    boost::geometry::transform(x_spherical_boost, x_boost);
    return CartesianCoordType{
        boost::geometry::get<0>(x_boost), boost::geometry::get<1>(x_boost), boost::geometry::get<2>(x_boost)};
  }
};

} // namespace Dune::XT::Data

#endif // DUNE_XT_DATA_COORDINATES_HH


/*
 *  Copyright (c) 2009, Rene Wagner
 *  All rights reserved.
 *
 *  Author: Rene Wagner <rw@nelianur.org>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Rene Wagner nor the names of any
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef __UKFOM_LAPACK_CHOLESKY_HPP__
#define __UKFOM_LAPACK_CHOLESKY_HPP__

#include "lapack.h"

#include <Eigen/Core>

namespace ukfom {
namespace lapack {
using namespace Eigen;

template<size_t M>
class cholesky
{
public:
	cholesky(const Matrix<double, M, M> &m)
	{
		L_ = m;

		char UPLO = 'L';
		int N = L_.cols();
		int LDA = L_.stride();
		int INFO;

		dpotrf_(&UPLO, &N, L_.data(), &LDA, &INFO);

		spd_ = INFO == 0;

		// clear everything but the lower triangular matrix
		for (int j = 1; j < L_.cols(); ++j)
			for (int i = 0; i < j; ++i)
				L_(i,j) = 0;
	}

	const Matrix<double, M, M> &getL() const
	{
		if (!spd_)
			throw "not SPD";
		return L_;
	}

	bool isSPD() const
	{
		return spd_;
	}

private:
	Matrix<double, M, M> L_;
	bool spd_;
};

} // namespace lapack
} // namespace ukfom

#endif // __UKFOM_LAPACK_CHOLESKY_HPP__


/* Boost example/horner.cpp
 * example of unprotecting rounding for a whole function computation
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

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

// I is an interval class, the polynom is a simple array
template<class I>
I horner(const I& x, const I p[], int n) {

  // initialize and restore the rounding mode
  typename I::traits_type::rounding rnd;

  // define the unprotected version of the interval type
  typedef typename boost::numeric::interval_lib::unprotect<I>::type R;

  const R& a = x;
  R y = p[n - 1];
  for(int i = n - 2; i >= 0; i--) {
    y = y * a + (const R&)(p[i]);
  }
  return y;

  // restore the rounding mode with the destruction of rnd
}

template<class T, class Policies>
std::ostream &operator<<(std::ostream &os,
                         const boost::numeric::interval<T, Policies> &x) {
  os << "[" << x.lower() << ", " << x.upper() << "]";
  return os;
}

int main() {
  typedef boost::numeric::interval<double> I;
  I p[3] = { -1.0, 0, 1.0 };
  I x = 1.0;
  std::cout << horner(x, p, 3) << std::endl;
  return 0;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, int_step_double) {
  using stan::math::int_step;
  EXPECT_EQ(0U, int_step(-1.0));
  EXPECT_EQ(0U, int_step(0.0));
  EXPECT_EQ(1U, int_step(0.00000000001));
  EXPECT_EQ(1U, int_step(100.0));
}

TEST(MathFunctions, int_step_int) {
  using stan::math::int_step;

  EXPECT_EQ(0U, int_step(static_cast<int>(-1)));
  EXPECT_EQ(0U, int_step(static_cast<int>(0)));
  EXPECT_EQ(1U, int_step(static_cast<int>(100)));
}

TEST(MathFunctions, int_step_inf) {
  using stan::math::int_step;

  EXPECT_EQ(1U, int_step(std::numeric_limits<double>::infinity()));
  EXPECT_EQ(0U, int_step(-std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, int_step_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_EQ(0U, stan::math::int_step(nan));
}