/*
   (c) Copyright 2012, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Interpolation search template
*/

#ifndef LINTEL_INTERPOLATION_SEARCH_HPP
#define LINTEL_INTERPOLATION_SEARCH_HPP

#include <inttypes.h>

#include <boost/type_traits/make_unsigned.hpp>

#include <Lintel/AssertBoost.hpp>

// http://www.cs.technion.ac.il/~itai/publications/Algorithms/p550-perl.pdf
// http://en.wikipedia.org/wiki/Interpolation_search

namespace lintel {

    // TODO: redo the interface to have no precondition, i.e. v does not have to be in first..last
    // and that if it is outside that rande, the function can return a value < 0 or > last_pos.
    // but it needs to not crash.  Then clamp the return in the caller using the standard signed
    // compare trick to usually only take one compare to check if 0..last_pos but if it is out of
    // range a second to decide whether we clamp to 0 or last_pos.  A good estimator will estimate
    // <0 if v < first, and >last_pos if v > last.  This approach means that we avoid the compares
    // to re-establish the pre-condition at the cost of compares to clamp the range, but the clamp
    // compares are always going to be fast because they are ssize_t compares, whereas the current
    // compares could be expensive because they are value compares.  Tricky question on what
    // happens if first == last --> value_range == 0.  That can't happen right now because it
    // implies the return value is first and we will have stopped, but with this change it can
    // happen.
template<typename ValueT> struct EstimateOffset {
    typedef typename boost::make_unsigned<ValueT>::type UnsignedValueT;
    // Preconditions: first < v <= last
    // Postconditions: 0 <= return value <= last_pos
    size_t
    operator()(const ValueT &v, const ValueT &first, const ValueT &last, size_t last_pos) const {
        UnsignedValueT value_range = last - first;
        double relative_pos = static_cast<double>(v - first) / value_range; // [0,1]
        return relative_pos * last_pos;
    }
};

template<typename ValueT> struct Compare {
    bool operator()(const ValueT &a, const ValueT &b) const {
        return a < b;
    }
};

namespace detail {

template<size_t interpolation_steps, size_t binary_steps, size_t binary_search_below_length,
         typename ValueT, typename Iterator, class EstimateOffsetT, class CompareT>
Iterator interpolationLowerBound(Iterator begin, Iterator end, const ValueT &v,
                                 const EstimateOffsetT &estimator, const CompareT &comparer) {
    size_t estimated_offset;
    Iterator estimated_pos;

    if (begin == end) {
        return begin;
    }
    if (comparer(*begin, v)) {
        if (comparer(*(end - 1), v)) {
            // v > *end
            return end;
        } else {
            // ok, v \in ]*begin, *end]
        }
    } else {
        return begin;
    }

    // estimateOffset function can now safely assume that v is in range, so we don't have
    // to check each time to see if we are in bounds.

    size_t len = end - begin; // - on struct iterator involves divide, do it once.
    while (len > 0) {
        for (size_t is = 0; len > 0 && is < interpolation_steps; ++is) {
            DEBUG_SINVARIANT(begin < end);
            DEBUG_SINVARIANT((end - begin) == static_cast<ptrdiff_t>(len));
            DEBUG_SINVARIANT(comparer(*begin, v)); // *begin < v
            DEBUG_INVARIANT(!comparer(*(end - 1), v),// v <= *(end - 1)
                            boost::format("%s < %s") % *(end - 1) % v);
            estimated_offset = estimator(v, *begin, *(end - 1), len - 1);
            //            std::cout << boost::format("len = %d, is = %d, eo = %d\n") % len % is % estimated_offset;
            estimated_pos = begin + estimated_offset;
            DEBUG_INVARIANT(estimated_offset < len, boost::format("%d >= %d from %s [%s .. %s]")
                            % estimated_offset % len % v % *begin % *(end - 1));
            DEBUG_SINVARIANT(estimated_pos < end);
            if (comparer(*estimated_pos, v)) {
                begin = estimated_pos;
                ++begin;
                if (!comparer(*begin, v)) {
                    return begin; // this is lower bound since it is not < and one less is
                }
                len = len - (estimated_offset + 1);
            } else { // estimated_pos >= v
                if (comparer(*(estimated_pos - 1), v)) {
                    return estimated_pos; // this is the lower bound since it is not < and one less is
                }
                end = estimated_pos;
                len = estimated_offset;
            }
        }

        if (len == 0) {
            return begin;
        }

        // std::cout << boost::format("len = %d\n") % len;
        if (len <= binary_search_below_length) {
            return std::lower_bound(begin, end, v, comparer);
        }
        bool adjusted_begin = false, adjusted_end = false;
        for (size_t bs = 0; len > 0 && bs < binary_steps; ++bs) {
            DEBUG_SINVARIANT(begin < end);
            DEBUG_SINVARIANT((end - begin) == static_cast<ptrdiff_t>(len));
            estimated_offset = len >> 1;
            estimated_pos = begin + estimated_offset;
            if (comparer(*estimated_pos, v)) {
                begin = estimated_pos;
                ++begin;
                len = len - (estimated_offset + 1);
                adjusted_begin = true;
            } else {
                len = estimated_offset;
                end = estimated_pos;
                adjusted_end = true;
            }
        }
        // Repair invariants used for interpolation function
        if (adjusted_begin) {
            if (!comparer(*begin, v)) {
                return begin;
            }
        }
        if (adjusted_end) {
            if (comparer(*(end - 1), v)) {
                return end;
            }
        }
    }
    return begin;
}

} // namespace detail

// We want to write estimator = EstimateOffset<ValueT>, but that requires C++0x
template<typename ValueT, typename Iterator> Iterator
interpolationLowerBound(Iterator begin, Iterator end, const ValueT &v) {
    EstimateOffset<ValueT> eo;
    return detail::interpolationLowerBound<2,4,256>(begin, end, v, eo, Compare<ValueT>());
}

} // namespace lintel

#endif


#include "quadrotor_simulator/Quadrotor.h"
#include <iostream>
#include <boost/bind.hpp>
#include "odeint-v2/boost/numeric/odeint.hpp"
#include <Eigen/Geometry>

namespace odeint = boost::numeric::odeint;

namespace QuadrotorSimulator
{

Quadrotor::Quadrotor(void)
{
  g_ = 9.81;
  mass_ = 0.075;
  double Ixx = 2.64e-3, Iyy = 2.64e-3, Izz = 4.96e-3;
  prop_radius_ = 0.099;
  J_ = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal();

  kf_ = 5.55e-8;
  // km_ = 2.5e-9; // from Nate
  // km = (Cq/Ct)*Dia*kf
  // Cq/Ct for 8 inch props from UIUC prop db ~ 0.07
  km_ = 0.07*(2*prop_radius_)*kf_;

  arm_length_ = 0.17;
  motor_time_constant_ = 1.0/20;
  min_rpm_ = 1500;
  max_rpm_ = 7500;

  state_.x = Eigen::Vector3d::Zero();
  state_.v = Eigen::Vector3d::Zero();
  state_.R = Eigen::Matrix3d::Identity();
  state_.omega = Eigen::Vector3d::Zero();
  state_.motor_rpm = Eigen::Array4d::Zero();
  updateInternalState();

  input_ = Eigen::Array4d::Zero();
}

void Quadrotor::step(double dt)
{
  odeint::integrate(boost::ref(*this), internal_state_, 0.0, dt, dt);

  for(int i = 0; i < 3; i++)
  {
    state_.x(i) = internal_state_[0+i];
    state_.v(i) = internal_state_[3+i];
    state_.R(i,0) = internal_state_[6+i];
    state_.R(i,1) = internal_state_[9+i];
    state_.R(i,2) = internal_state_[12+i];
    state_.omega(i) = internal_state_[15+i];
  }
  state_.motor_rpm(0) = internal_state_[18];
  state_.motor_rpm(1) = internal_state_[19];
  state_.motor_rpm(2) = internal_state_[20];
  state_.motor_rpm(3) = internal_state_[21];

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(state_.R.transpose()*state_.R);
  Eigen::Matrix3d P = llt.matrixL();
  Eigen::Matrix3d R = state_.R*P.inverse();
  state_.R = R;

  // Don't go below zero, simulate floor
  if(state_.x(2) < 0.0 && state_.v(2) < 0)
  {
    state_.x(2) = 0;
    state_.v(2) = 0;
  }
  updateInternalState();
}

void Quadrotor::operator()(const Quadrotor::InternalState &x, Quadrotor::InternalState &dxdt, const double /* t */)
{
  State cur_state;
  for(int i = 0; i < 3; i++)
  {
    cur_state.x(i) = x[0+i];
    cur_state.v(i) = x[3+i];
    cur_state.R(i,0) = x[6+i];
    cur_state.R(i,1) = x[9+i];
    cur_state.R(i,2) = x[12+i];
    cur_state.omega(i) = x[15+i];
  }
  for(int i = 0; i < 4; i++)
  {
    cur_state.motor_rpm(i) = x[18 + i];
  }

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(cur_state.R.transpose()*cur_state.R);
  Eigen::Matrix3d P = llt.matrixL();
  Eigen::Matrix3d R = cur_state.R*P.inverse();

  Eigen::Vector3d x_dot, v_dot, omega_dot;
  Eigen::Matrix3d R_dot;
  Eigen::Array4d motor_rpm_dot;
  Eigen::Array4d motor_rpm_sq;
  Eigen::Matrix3d omega_hat(Eigen::Matrix3d::Zero());

  omega_hat(2,1) = cur_state.omega(0);
  omega_hat(1,2) = -cur_state.omega(0);
  omega_hat(0,2) = cur_state.omega(1);
  omega_hat(2,0) = -cur_state.omega(1);
  omega_hat(1,0) = cur_state.omega(2);
  omega_hat(0,1) = -cur_state.omega(2);

  motor_rpm_sq = cur_state.motor_rpm.square();

  double thrust = kf_*motor_rpm_sq.sum();
  Eigen::Vector3d moments;
  moments(0) = kf_*(motor_rpm_sq(2) - motor_rpm_sq(3)) * arm_length_;
  moments(1) = kf_*(motor_rpm_sq(1) - motor_rpm_sq(0)) * arm_length_;
  moments(2) = km_*(motor_rpm_sq(0) + motor_rpm_sq(1) - motor_rpm_sq(2) - motor_rpm_sq(3));

  x_dot = cur_state.v;
  v_dot = -Eigen::Vector3d(0,0,g_) + thrust*R.col(2)/mass_ + external_force_/mass_;
  R_dot = R*omega_hat;
  omega_dot = J_.inverse()*(moments - cur_state.omega.cross(J_*cur_state.omega) + external_moment_);
  motor_rpm_dot = (input_ - cur_state.motor_rpm)/motor_time_constant_;

  for(int i = 0; i < 3; i++)
  {
    dxdt[0+i] = x_dot(i);
    dxdt[3+i] = v_dot(i);
    dxdt[6+i] = R_dot(i,0);
    dxdt[9+i] = R_dot(i,1);
    dxdt[12+i] = R_dot(i,2);
    dxdt[15+i] = omega_dot(i);
  }
  for(int i = 0; i < 4; i++)
  {
    dxdt[18 + i] = motor_rpm_dot(i);
  }
}

void Quadrotor::setInput(double u1, double u2, double u3, double u4)
{
  input_(0) = u1;
  input_(1) = u2;
  input_(2) = u3;
  input_(3) = u4;
  for(int i = 0; i < 4; i++)
  {
    if(input_(i) > max_rpm_)
      input_(i) = max_rpm_;
    else if(input_(i) < min_rpm_)
      input_(i) = min_rpm_;
  }
}

const Quadrotor::State &Quadrotor::getState(void) const
{
  return state_;
}
void Quadrotor::setState(const Quadrotor::State &state)
{
  state_.x = state.x;
  state_.v = state.v;
  state_.R = state.R;
  state_.omega = state.omega;
  state_.motor_rpm = state.motor_rpm;

  updateInternalState();
}

double Quadrotor::getMass(void) const
{
  return mass_;
}
void Quadrotor::setMass(double mass)
{
  mass_ = mass;
}

double Quadrotor::getGravity(void) const
{
  return g_;
}
void Quadrotor::setGravity(double g)
{
  g_ = g;
}

const Eigen::Matrix3d &Quadrotor::getInertia(void) const
{
  return J_;
}
void Quadrotor::setInertia(const Eigen::Matrix3d &inertia)
{
  if(inertia != inertia.transpose())
  {
    std::cerr << "Inertia matrix not symmetric, not setting" << std::endl;
    return;
  }
  J_ = inertia;
}

double Quadrotor::getArmLength(void) const
{
  return arm_length_;
}
void Quadrotor::setArmLength(double d)
{
  if(d <= 0)
  {
    std::cerr << "Arm length <= 0, not setting" << std::endl;
    return;
  }

  arm_length_ = d;
}

double Quadrotor::getPropRadius(void) const
{
  return prop_radius_;
}
void Quadrotor::setPropRadius(double r)
{
  if(r <= 0)
  {
    std::cerr << "Prop radius <= 0, not setting" << std::endl;
    return;
  }
  prop_radius_ = r;
}

double Quadrotor::getPropellerThrustCoefficient(void) const
{
  return kf_;
}
void Quadrotor::setPropellerThrustCoefficient(double kf)
{
  if(kf <= 0)
  {
    std::cerr << "Thrust coefficient <= 0, not setting" << std::endl;
    return;
  }

  kf_ = kf;
}

double Quadrotor::getPropellerMomentCoefficient(void) const
{
  return km_;
}
void Quadrotor::setPropellerMomentCoefficient(double km)
{
  if(km <= 0)
  {
    std::cerr << "Moment coefficient <= 0, not setting" << std::endl;
    return;
  }

  km_ = km;
}

double Quadrotor::getMotorTimeConstant(void) const
{
  return motor_time_constant_;
}
void Quadrotor::setMotorTimeConstant(double k)
{
  if(k <= 0)
  {
    std::cerr << "Motor time constant <= 0, not setting" << std::endl;
    return;
  }

  motor_time_constant_ = k;
}

const Eigen::Vector3d &Quadrotor::getExternalForce(void) const
{
  return external_force_;
}
void Quadrotor::setExternalForce(const Eigen::Vector3d &force)
{
  external_force_ = force;
}

const Eigen::Vector3d &Quadrotor::getExternalMoment(void) const
{
  return external_moment_;
}
void Quadrotor::setExternalMoment(const Eigen::Vector3d &moment)
{
  external_moment_ = moment;
}

double Quadrotor::getMaxRPM(void) const
{
  return max_rpm_;
}
void Quadrotor::setMaxRPM(double max_rpm)
{
  if(max_rpm <= 0)
  {
    std::cerr << "Max rpm <= 0, not setting" << std::endl;
    return;
  }
  max_rpm_ = max_rpm;
}

double Quadrotor::getMinRPM(void) const
{
  return min_rpm_;
}
void Quadrotor::setMinRPM(double min_rpm)
{
  if(min_rpm < 0)
  {
    std::cerr << "Min rpm < 0, not setting" << std::endl;
    return;
  }
  min_rpm_ = min_rpm;
}

void Quadrotor::updateInternalState(void)
{
  for(int i = 0; i < 3; i++)
  {
    internal_state_[0+i] = state_.x(i);
    internal_state_[3+i] = state_.v(i);
    internal_state_[6+i] = state_.R(i,0);
    internal_state_[9+i] = state_.R(i,1);
    internal_state_[12+i] = state_.R(i,2);
    internal_state_[15+i] = state_.omega(i);
  }
  internal_state_[18] = state_.motor_rpm(0);
  internal_state_[19] = state_.motor_rpm(1);
  internal_state_[20] = state_.motor_rpm(2);
  internal_state_[21] = state_.motor_rpm(3);
}

}


// Std includes
#include <cmath>
#include <iostream>
#include <memory>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
#include "sl0/chain/static.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// State
template<int StateSize>
using TypeState = Eigen::Matrix<TypeScalar, StateSize, 1>;
using TypeStateDynamic = Eigen::Matrix<TypeScalar, Eigen::Dynamic, 1>;
// Space
constexpr unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Group Parameters
using TypeStepPoint = sl0::StepPoint<TypeState, DIM, TypeRef, TypeView, Flow>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg;

const unsigned int np = 10;

int main () {
    // Parameters
    TypeVector x0 = TypeVector({0.0, 0.0, 0.0});
    TypeScalar t0 = 0.0;
    TypeScalar dt = 1e-3;
    unsigned int nt = std::round(1.0 / dt);
    double l = 1.0;
    // Create chain
    std::shared_ptr<TypeStepPoint> sStepPoint = std::make_shared<TypeStepPoint>(std::make_shared<Flow>());
    sl0::ChainStatic<TypeState, DIM, TypeRef, np, TypeView, TypeStepPoint, TypeSolver> chain(sStepPoint, l, 4);
    // Init
    for(std::size_t i = 0; i < np; i++) {
        sStepPoint->x(chain.sStep->memberState(chain.state, i)) = x0;
        sStepPoint->x(chain.sStep->memberState(chain.state, i))[0] += i * l/double(np-1);
    }
    std::cout << "Init Length : " << "\n" << chain.sStep->actualLength(chain.state) << "\n";
    std::cout << "Init State : " << "\n" << chain.state << "\n";
    chain.t = t0;
    // Computation
    std::cout << "Computing" << "\n";
    for(std::size_t i = 0; i < nt; i++) {
        chain.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Chain advected following an exponential flow, exp(" << chain.t << ") = " << "\n";
    std::cout << "\n";
    std::cout << "Final State : " << "\n" << chain.state << "\n";
    std::cout << "Final Length : " << "\n" << chain.sStep->actualLength(chain.state) << "\n";
    std::cout << std::endl;
}


#include "TensorCoreLinesEvaluator.hh"

#include <Eigen/Geometry>

#include <boost/algorithm/cxx11/any_of.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/algorithm/max_element.hpp>

#include <vector>
#include <tuple>
#include <utility>

using namespace cpp_utils;

namespace tl
{

template <typename T, std::size_t... Degrees>
using TPBT = TensorProductBezierTriangle<T, double, Degrees...>;

std::pair<std::array<TPBT<double, 1, 2>, 3>, std::array<TPBT<double, 0, 3>, 3>>
tensorCoreLinesCoeffs(const TensorInterp& t,
                      const std::array<TensorInterp, 3>& dt,
                      const Triangle& r)
{
    using Coords1 = TPBT<double, 1, 2>::Coords;
    using Coords2 = TPBT<double, 0, 3>::Coords;

    // (T * r) x r
    auto eval_ev =
            [&](const Coords1& coords, const TensorInterp& t, int i) -> double {
        auto rv = r(coords.tail<3>());
        auto tv = t(coords.head<3>());
        return (tv * rv).cross(rv)[i];
    };

    // ((\nabla T * r) * r) x r
    auto eval_deriv_ev = [&](const Coords2& coords,
                             const std::array<TensorInterp, 3>& dt,
                             int i) -> double {
        auto rv = r(coords.tail<3>());
        auto txv = dt[0](coords.head<3>());
        auto tyv = dt[1](coords.head<3>());
        auto tzv = dt[2](coords.head<3>());

        return ((txv * rv[0] + tyv * rv[1] + tzv * rv[2]) * rv).cross(rv)[i];
    };

    auto eval1 = [&](const Coords1& coords) { return eval_ev(coords, t, 0); };
    auto eval2 = [&](const Coords1& coords) { return eval_ev(coords, t, 1); };
    auto eval3 = [&](const Coords1& coords) { return eval_ev(coords, t, 2); };

    auto eval4 = [&](const Coords2& coords) {
        return eval_deriv_ev(coords, dt, 0);
    };
    auto eval5 = [&](const Coords2& coords) {
        return eval_deriv_ev(coords, dt, 1);
    };
    auto eval6 = [&](const Coords2& coords) {
        return eval_deriv_ev(coords, dt, 2);
    };

    return {{TPBT<double, 1, 2>{eval1},
             TPBT<double, 1, 2>{eval2},
             TPBT<double, 1, 2>{eval3}},
            {TPBT<double, 0, 3>{eval4},
             TPBT<double, 0, 3>{eval5},
             TPBT<double, 0, 3>{eval6}}};
}

using TSHE = TensorCoreLinesEvaluator;

TSHE::TensorCoreLinesEvaluator(const DoubleTri& tri,
                               const TensorInterp& t,
                               const std::array<TensorInterp, 3>& dt,
                               const Options& opts)
        : _tri(tri),
          _opts(opts)
{
    std::tie(_target_funcs_t, _target_funcs_dt) =
            tensorCoreLinesCoeffs(t, dt, tri.dir_tri);
}

std::array<TSHE, 4> TSHE::split() const
{
    if(_last_split_dir)
    {
        return split<0>();
    }
    return split<1>();
}

Result TSHE::eval()
{
    // Check if any of the error components can not become zero in the
    // current subdivision triangles
    auto has_nonzero =
            boost::algorithm::any_of(_target_funcs_t, [](const auto& c) {
                    return sameSign(c.coefficients()) != 0;
            })
            || boost::algorithm::any_of(_target_funcs_dt, [](const auto& c) {
                   return sameSign(c.coefficients()) != 0;
               });

    // Discard triangles if no roots can occur inside
    if(has_nonzero)
    {
        return Result::Discard;
    }

    // Compute upper bound for target functions
    auto max_error = std::max(abs_max_upper_bound(_target_funcs_t),
                              abs_max_upper_bound(_target_funcs_dt));

    if(max_error < _opts.tolerance)
    {
        return Result::Accept;
    }

    return Result::Split;
}

double TSHE::error() const
{
    return std::max(upper_bound_norm(_target_funcs_t),
                    upper_bound_norm(_target_funcs_dt));
}

double distance(const TSHE& t1, const TSHE& t2)
{
    return distance(t1.tris(), t2.tris());
}

bool operator==(const TSHE& t1, const TSHE& t2)
{
    return t1._tri == t2._tri && t1._target_funcs_t == t2._target_funcs_t
           && t1._target_funcs_dt == t2._target_funcs_dt
           && t1._last_split_dir == t2._last_split_dir
           && t1._split_level == t2._split_level && t1._opts == t2._opts;
}

bool operator!=(const TSHE& t1, const TSHE& t2)
{
    return !(t1 == t2);
}

}


#include <boost/math/distributions/weibull.hpp>


/** @file
    @brief Implementation

    @date 2015

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// Copyright 2015 Sensics, Inc.
//
// SPDX-License-Identifer: Apache-2.0
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
template <typename T>
inline void dumpKalmanDebugOuput(const char name[], const char expr[],
                                 T const &value) {
    std::cout << "\n(Kalman Debug Output) " << name << " [" << expr << "]:\n"
              << value << std::endl;
}

#define FLEXKALMAN_DEBUG_OUTPUT(Name, Value)                                   \
    dumpKalmanDebugOuput(Name, #Value, Value)

// Internal Includes
#include "FlexKalman/AbsoluteOrientationMeasurement.h"
#include "FlexKalman/FlexibleKalmanFilter.h"
#include "FlexKalman/PoseConstantVelocity.h"

#include "ContentsInvalid.h"

// Library/third-party includes
// - none

// Standard includes
#include <iostream>

int main() {
    using ProcessModel = flexkalman::PoseConstantVelocityProcessModel;
    using State = flexkalman::pose_externalized_rotation::State;
    using Measurement = flexkalman::AbsoluteOrientationEKFMeasurement<State>;
    State state;
    ProcessModel processModel;
    std::cout << "Initial state:" << std::endl;
    std::cout << state << std::endl;
    {
        auto meas = Measurement{Eigen::Quaterniond::Identity(),
                                Eigen::Vector3d(0.00001, 0.00001, 0.00001)};
        std::cout << "Measurement covariance:\n"
                  << meas.getCovariance(state) << std::endl;

        for (int i = 0; i < 100; ++i) {
            flexkalman::predict(state, processModel, 0.1);

            std::cout << "\nAfter prediction (iteration " << i << "):\n"
                      << state << std::endl;
            if (stateContentsInvalid(state)) {
                std::cout << "ERROR: Detected invalid state contents after "
                             "prediction step of iteration "
                          << i << std::endl;
                return -1;
            }
            if (covarianceContentsInvalid(state)) {
                std::cout << "ERROR: Detected invalid covariance contents "
                             "after prediction step of iteration "
                          << i << std::endl;
                return -1;
            }
            flexkalman::correct(state, processModel, meas);
            std::cout << "\nAfter correction (iteration " << i << "):\n"
                      << state << std::endl;
            if (stateContentsInvalid(state)) {
                std::cout << "ERROR: Detected invalid state contents after "
                             "correction step of iteration "
                          << i << std::endl;
                return -1;
            }
            if (covarianceContentsInvalid(state)) {
                std::cout << "ERROR: Detected invalid covariance contents "
                             "after correction step of iteration "
                          << i << std::endl;
                return -1;
            }
        }
    }
    std::cout << "SUCCESS: ran till completion of configured number of "
                 "iterations without denormalizing in some way."
              << std::endl;
    return 0;
}


#include "mLibInclude.h"
#include "../../shared/OptUtils.h"
#define GLOG_NO_ABBREVIATED_SEVERITIES
#include <Eigen33b1/Eigen>
#include <Eigen33b1/IterativeLinearSolvers>
#include <cuda_runtime.h>

#include <time.h>

#define LEAST_SQ_CONJ_GRADIENT 0
#define SPARSE_QR 1

#define SOLVER LEAST_SQ_CONJ_GRADIENT

typedef Eigen::Triplet<float> Tripf;
typedef Eigen::SparseMatrix<float> SpMatrixf;
#include "EigenSolverPoissonImageEditing.h"
#include <Eigen33b1/OrderingMethods>

#if SOLVER == LEAST_SQ_CONJ_GRADIENT
typedef Eigen::LeastSquaresConjugateGradient<SpMatrixf > AxEqBSolver;
#elif SOLVER == SPARSE_QR
typedef Eigen::SparseQR<SpMatrixf, Eigen::COLAMDOrdering<int> > AxEqBSolver;
#endif

struct vec2iHash {
    size_t operator()(const vec2i& v) const {
        return std::hash < int > {}(v.x) ^ std::hash < int > {}(v.y);
    }
};

float4 sampleImage(float4* image, vec2i p, int W) {
    return image[p.y*W + p.x];
}

void setPixel(float4* image, vec2i p, int W, float r, float g, float b) {
    image[p.y*W + p.x].x = r;
    image[p.y*W + p.x].y = g;
    image[p.y*W + p.x].z = b;
}

void solveAxEqb(AxEqBSolver& solver, const Eigen::VectorXf& b, Eigen::VectorXf& x) {

#if SOLVER==LEAST_SQ_CONJ_GRADIENT
    x = solver.solveWithGuess(b, x);
    //std::cout << "#iterations:     " << solver.iterations() << std::endl;
    //std::cout << "estimated error: " << solver.error() << std::endl;
#else
    x = solver.solve(b);
#endif
}
double EigenSolverPoissonImageEditing::solve(const NamedParameters& solverParameters, const NamedParameters& problemParameters, bool profileSolve, std::vector<SolverIteration>& iters)
{
    int numUnknowns = 0;
    std::unordered_map<vec2i, int, vec2iHash> pixelLocationsToIndex;
    std::vector<vec2i> pixelLocations;
    size_t pixelCount = m_dims[0] * m_dims[1];
    std::vector<float4> h_unknownFloat(pixelCount);
    std::vector<float4> h_target(pixelCount);
    std::vector<float>  h_mask(pixelCount);

    findAndCopyArrayToCPU("X", h_unknownFloat, problemParameters);
    findAndCopyArrayToCPU("T", h_target, problemParameters);
    findAndCopyArrayToCPU("M", h_mask, problemParameters);

    for (int y = 0; y < (int)m_dims[1]; ++y) {
        for (int x = 0; x < (int)m_dims[0]; ++x) {
            if (h_mask[y*m_dims[0] + x] == 0.0f) {
                ++numUnknowns;
                vec2i p(x, y);
                pixelLocationsToIndex[p] =(int)pixelLocations.size();
                pixelLocations.push_back(p);
            }
        }
    }
    printf("# Unknowns: %d\n", numUnknowns);
    int numResiduals = (int)pixelLocations.size() * 4;

    Eigen::VectorXf x_r(numUnknowns), b_r(numResiduals);
    Eigen::VectorXf x_g(numUnknowns), b_g(numResiduals);
    Eigen::VectorXf x_b(numUnknowns), b_b(numResiduals);
    Eigen::VectorXf x_a(numUnknowns), b_a(numResiduals);

    b_r.setZero();
    b_g.setZero();
    b_b.setZero();
    b_a.setZero();

    for (int i = 0; i < pixelLocations.size(); ++i) {
        vec2i p = pixelLocations[i];
        float4 color = sampleImage(h_unknownFloat.data(), p, m_dims[0]);
        x_r[i] = color.x;
        //printf("%f\n", color.x);
        x_g[i] = color.y;
        x_b[i] = color.z;
        x_a[i] = color.w;
    }
    SpMatrixf A(numResiduals, numUnknowns);
    A.setZero();
    printf("Constructing Matrix\n");
    std::vector<Tripf> entriesA;

    std::vector<vec2i> offsets;
    offsets.push_back(vec2i(-1, 0));
    offsets.push_back(vec2i(1, 0));
    offsets.push_back(vec2i(0, -1));
    offsets.push_back(vec2i(0, 1));

    for (int i = 0; i < pixelLocations.size(); ++i) {
        vec2i p = pixelLocations[i];
        int numInternalNeighbors = 0;
        float4 g_p = sampleImage(h_target.data(), p, m_dims[0]);
        int j = 0;

        for (vec2i off : offsets) {
            vec2i q = p + off;
            if (q.x >= 0 && q.y >= 0 && q.x < (int)m_dims[0] && q.y < (int)m_dims[1]) {
                auto it = pixelLocationsToIndex.find(q);
                int row = 4 * i + j;
                if (it == pixelLocationsToIndex.end()) {
                    float4 f_q = sampleImage(h_unknownFloat.data(), q, m_dims[0]);
                    b_r[row] += f_q.x;
                    b_g[row] += f_q.y;
                    b_b[row] += f_q.z;
                    b_a[row] += f_q.w;
                }
                else {
                    entriesA.push_back(Tripf(row, it->second, -1.0f));
                }
                entriesA.push_back(Tripf(row, i, 1.0f));

                float4 g_q = sampleImage(h_target.data(), q, m_dims[0]);
                b_r[row] += (g_p.x - g_q.x);
                b_g[row] += (g_p.y - g_q.y);
                b_b[row] += (g_p.z - g_q.z);
                b_a[row] += (g_p.w - g_q.w);
            }
            ++j;

        }
    }

    printf("Entries Set\n");
    A.setFromTriplets(entriesA.begin(), entriesA.end());
    printf("Sparse Matrix Constructed\n");
    A.makeCompressed();
    printf("Matrix Compressed\n");
    {
        float totalCost = 0.0f;

        float cost_r = (A*x_r - b_r).squaredNorm();
        float cost_g = (A*x_g - b_g).squaredNorm();
        float cost_b = (A*x_b - b_b).squaredNorm();
        float cost_a = (A*x_a - b_a).squaredNorm();
        totalCost = cost_r + cost_g + cost_b + cost_a;
        printf("Initial Cost: %f : (%f, %f, %f, %f)\n", totalCost, cost_r, cost_g, cost_b, cost_a);

    }

    AxEqBSolver solver;
    solver.setMaxIterations(97);
    printf("Solvers Initialized\n");

    clock_t start = clock(), diff;

    solver.compute(A);
    //printf("solver.compute(A)\n");
    solveAxEqb(solver, b_r, x_r);
    //printf("Red solve done\n");
    solveAxEqb(solver, b_g, x_g);
    //printf("Green solve done\n");
    solveAxEqb(solver, b_b, x_b);
    //printf("Blue solve done\n");
    solveAxEqb(solver, b_a, x_a);

    diff = clock() - start;
    printf("Time taken %f ms\n", diff*1000.0 / double(CLOCKS_PER_SEC));

    float totalCost = 0.0f;

    float cost_r = (A*x_r - b_r).squaredNorm();
    float cost_g = (A*x_g - b_g).squaredNorm();
    float cost_b = (A*x_b - b_b).squaredNorm();
    float cost_a = (A*x_a - b_a).squaredNorm();
    totalCost = cost_r + cost_g + cost_b + cost_a;
    printf("Final Cost: %f : (%f, %f, %f, %f)\n", totalCost, cost_r, cost_g, cost_b, cost_a);

    for (int i = 0; i < pixelLocations.size(); ++i) {
        setPixel(h_unknownFloat.data(), pixelLocations[i], m_dims[0], x_r[i], x_g[i], x_b[i]);
    }
    findAndCopyToArrayFromCPU("X", h_unknownFloat, problemParameters);;
    return (double)totalCost;

}

/* Proper Poisson Image Editing

for (int i = 0; i < pixelLocations.size(); ++i) {
vec2i p = pixelLocations[i];
int row = i;
int numInternalNeighbors = 0;
float4 g_p = sampleImage(h_target, p, m_width);
for (int off_y = -1; off_y <= 1; off_y += 2) {
for (int off_x = -1; off_x <= 1; off_x += 2) {
vec2i q(p.x + off_x, p.y + off_y);
auto it = pixelLocationsToIndex.find(q);
if (it != pixelLocationsToIndex.end()) {
++numInternalNeighbors;
entriesA.push_back(Tripf(row, it->second, -1.0f));
} else {

float4 f_star_q = sampleImage(h_target, q, m_width);
b_r[i] += f_star_q.x;
b_g[i] += f_star_q.y;
b_b[i] += f_star_q.z;
}
float4 g_q = sampleImage(h_target, q, m_width);
b_r[i] += (g_p.x - g_q.x);
b_g[i] += (g_p.y - g_q.y);
b_b[i] += (g_p.z - g_q.z);
}
}
entriesA.push_back(Tripf(row, row, (float)numInternalNeighbors));
}

*/


#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/tools/config/SelfConfig.h>
#include <limits>
#include <boost/function.hpp>
#include <boost/bind.hpp>

#include "pid_ctrl.h"

namespace ob = ompl::base;
namespace oc = ompl::control;

ompl::control::PID::PID(const SpaceInformationPtr &si) : base::Planner(si, "PID")
{
	specs_.approximateSolutions = true;
	siC_ = si.get();
}

ompl::control::PID::~PID(void)
{
}

void ompl::control::PID::setup(void)
{
	base::Planner::setup();
}

void ompl::control::PID::clear(void)
{
	Planner::clear();
	sampler_.reset();
	controlSampler_.reset();
}

void ompl::control::PID::control_action(oc::Control*& control, ob::State*& state)
{
	static const double KP = 25.0;
	static const double KD = 5.0;
	//static const double KP = 0.0;
	//static const double KD = 0.0;
	double* u = control->as<oc::RealVectorControlSpace::ControlType>()->values;

	const ompl::base::CompoundState* cstate = state->as<ompl::base::CompoundState>();
	const ompl::base::SO2StateSpace::StateType* theta = cstate->as<ompl::base::SO2StateSpace::StateType>(1);
	const ompl::base::SO2StateSpace::StateType* omega = cstate->as<ompl::base::SO2StateSpace::StateType>(3);

	*u = KP * theta->value + KD * omega->value;
}
ompl::base::PlannerStatus ompl::control::PID::solve(const base::PlannerTerminationCondition &ptc)
{
	checkValidity();
	base::Goal                   *goal = pdef_->getGoal().get();

	while (const base::State *st = pis_.nextStart())
	{
		motions.push_back(new Motion(siC_));
		si_->copyState(motions.back()->state, st);
		siC_->nullControl(motions.back()->control);
	}

	if(motions.empty())
	{
		OMPL_ERROR("Invalid Start State");
		return base::PlannerStatus::INVALID_START;
	}

	if (!sampler_)
	{
		sampler_ = si_->allocStateSampler();
	}

	if (!controlSampler_)
	{
		controlSampler_ = siC_->allocDirectedControlSampler();
	}

	bool solved = false;
	double  approxdif = std::numeric_limits<double>::infinity();
	while (ptc == false)
	{
		/* create a motion */
		// Add PID control action
		Motion* new_motion = new Motion(siC_);
		new_motion->state = si_->allocState();
		siC_->nullControl(new_motion->control);
		control_action(new_motion->control, motions.back()->state);

		// Add State Propagation
		unsigned int cd = siC_->propagateWhileValid(motions.back()->state, new_motion->control, siC_->getMinControlDuration(), new_motion->state);
		motions.push_back(new_motion);

		double dist = 0.0;
		bool solv = goal->isSatisfied(new_motion->state, &dist);
		if(solv)
		{
			solved = true;
		}

		if(dist < approxdif)
		{
			approxdif = dist;
		}
	}

	/* set the solution path */
	PathControl *path = new PathControl(si_);
	for (unsigned i = 0 ; i < motions.size()-1; ++i)
	{
		path->append(motions[i]->state, motions[i]->control, siC_->getPropagationStepSize());
	}
	path->append(motions[motions.size()-1]->state);
	pdef_->addSolutionPath(base::PathPtr(path), false, approxdif);
	return base::PlannerStatus(true, solved);
}

void ompl::control::PID::getPlannerData(base::PlannerData &data) const
{
	Planner::getPlannerData(data);
	double delta = siC_->getPropagationStepSize();

	if (motions.back())
	{
		data.addGoalVertex(base::PlannerDataVertex(motions.back()->state));
	}

	for (int i = motions.size()-1 ; i >= 1 ; --i)
	{
		const Motion* m = motions[i];
		data.addEdge(base::PlannerDataVertex(motions[i-1]->state), base::PlannerDataVertex(m->state), control::PlannerDataEdgeControl(m->control, delta));
	}
	data.addStartVertex(base::PlannerDataVertex(motions.front()->state));
}


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#pragma once

#include "libvideostitch/quaternion.hpp"

#include <Eigen/Dense>

#include <vector>

namespace VideoStitch {

namespace Stabilization {

typedef Eigen::Matrix<double, 9, 1> Vector9d;

/**
 * @brief The UKF_Quaternion class
 *
 * Inspired by: A Quaternion-Based Unscented Kalman Filter for Orientation Tracking (Edgar Kraft)
 */
class UKF_Quaternion {
 public:
  UKF_Quaternion();
  /**
   * @brief Propagate current state in time
   * @param delta_t : time in seconds
   */
  void predict(double delta_t);

  /**
   * @brief Incorporate a new measurement, update state and covariance matrix
   * @param v : 9 dimensional vector (gyr_xyz, acc_xyz, mag_xyz)
   */
  void measure(const Vector9d& v);

  Quaternion<double> getCurrentOrientation() const;
  Vector3<double> getCurrentAngularVelocity() const;
  Vector3<double> getCurrentAngularAcceleration() const;
  Eigen::Matrix<double, 9, 9> getCovarianceMatrix() const;

 protected:
  class qState {
   public:
    qState();
    void initFromVect(const Vector9d& v);
    void toVect(Vector9d& v) const;

    Quaternion<double> q;  // quaternion orientation
    Vector3<double> w;     // angular velocity
    Vector3<double> ww;    // angular acceleration
  };

  void computeSigmaPoints();
  void computeProcessModel(double delta_t);
  void computeAverageState();
  void computeCovarianceState();
  void computeMeasurements();
  void computeMeanAndCovMeasures();

  qState x;    // current a posteriori state vector
  qState xk_;  // a priori state vector

  Quaternion<double> b;  // orientation of the magnetic north
  Quaternion<double> g;  // orientation of the gravity

  Eigen::Matrix<double, 9, 9> P;    // current covariance of the state vector
  Eigen::Matrix<double, 9, 9> Pk_;  // a priori state vector covariance

  Eigen::Matrix<double, 9, 9> Q;  // process noise
  Eigen::Matrix<double, 9, 9> R;  // measurement noise

  Eigen::Matrix<double, 4, 18> M;  // temporary data used to compute Cholesky decomposition

  Eigen::Matrix<double, 9, 18> W_;  // temporary data used to compute a priori state vector covariance
  std::vector<Vector9d> W;          // temporary data used to compute sigma points
  std::vector<qState> X;            // sigma points {Xi}
  std::vector<qState> Y;            // propagated sigma points {Yi}
  Eigen::Matrix<double, 9, 18> Z;   // projection of the sigma points in the measurement space {Zi}
  std::vector<Vector3<double> > E;  // temporary data used to compute error vectors ei
  Vector9d zk_;                     // mean of {Zi}
  Eigen::Matrix<double, 9, 9> Pvv;  // innovation covariance
  Eigen::Matrix<double, 9, 9>
      Pxz;  // cross-correlation matrix (temporary data used for the computation of the Kalman gain)
  Eigen::Matrix<double, 9, 9> Kk;  // Kalman gain
};

}  // namespace Stabilization
}  // namespace VideoStitch


//
//! Copyright © 2022
//! Brandon Kohn
//
//  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)
//
#pragma once

#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <geometrix/utility/tagged_quantity.hpp>
#include <geometrix/utility/tagged_quantity_cmath.hpp>
#include <type_traits>

namespace geometrix {

    struct abs_tag{};
    struct ceil_tag{};
    struct floor_tag{};
    struct exp_tag{};
    struct log_tag{};
    struct log10_tag{};
    struct sqrt_tag{};
    struct cos_tag{};
    struct sin_tag{};
    struct tan_tag{};
    struct acos_tag{};
    struct asin_tag{};
    struct atan_tag{};
    struct hypot_tag{};
    struct pow_tag{};
    struct atan2_tag{};

    template <typename Tag, typename T1, typename T2=void, typename T3=void, typename T4=void, typename EnableIF=void>
    struct math_function_traits;

    template <typename Tag, typename T>
    struct math_function_traits<Tag, T, typename std::enable_if<std::is_floating_point<T>::value>::type>
    {
        using result_type = typename std::decay<T>::type;
    };

    template <typename Tag, typename T>
    struct math_function_traits<Tag, T, typename std::enable_if<std::is_integral<T>::value>::type>
    {
        using result_type = double;
    };

}//! namespace geometrix;


#include <boost/timer/timer.hpp>
#include <iostream>
#include <random>
#include <cmath>

using namespace std;

int main() {

	boost::timer::cpu_timer timer;
	int count = 1000*1000;
	random_device rd;
	minstd_rand minstd_rng(rd());
	mt19937 mt_rng(rd());
	normal_distribution<double> ndist(0,100);
	uniform_real_distribution<double> udist(-1, 1);

	// min max
	cout << "--- modf" << endl;
	cout << "------ modf() " << count << endl;
	{
		double f;
		double integral_part;
		double fractional_part;

		timer.start();
		for ( int i = 0; i< count; ++i ) {
			f = udist(minstd_rng);
			fractional_part = modf( f, &integral_part );
		}
		timer.stop();

		cout << "f               = " << f << " ( " << std::hexfloat << f << " )" << std::defaultfloat << endl;
		cout << "integral_part   = " << integral_part << " ( " << std::hexfloat << integral_part << " )" << std::defaultfloat <<endl;
		cout << "fractional_part = " << fractional_part << " ( " << std::hexfloat << fractional_part << " )" << std::defaultfloat <<endl;

		cout << "modf() " << timer.format() << endl;
	}

	return 0;
}


#include <cmath>
#include <iostream>
#include <limits>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/move/unique_ptr.hpp>

using namespace std;
namespace bpt = boost::posix_time;
namespace bml = boost::movelib;

const size_t LOOP_NUM = 100000000;

#define COUT_INTEGRAL(_t) \
    cout << "Size of " << #_t << " = " << sizeof(_t) << ", " \
         << "[ " << numeric_limits<_t>::min() << ", " \
         << numeric_limits<_t>::max() << " ]." << endl;

#define COUT_FLOATING_POINT(_t) \
    cout << "Size of " << #_t << " = " << sizeof(_t) << ", " \
         << scientific << "[ " << numeric_limits<_t>::min() << ", " \
         << numeric_limits<_t>::max() << " ]." << endl;

static void show_types(void)
{
    COUT_INTEGRAL(int);
    COUT_INTEGRAL(short);
    COUT_INTEGRAL(long);

    COUT_FLOATING_POINT(float);
    COUT_FLOATING_POINT(double);
    COUT_FLOATING_POINT(long double);
}

class Runnable
{
public:
    Runnable() { };
    virtual ~Runnable() { };

    virtual void prepare(void) = 0;
    virtual void run(int loops) = 0;
    virtual void finalize(void) = 0;
};

template <typename _T>
class FloatingPointMuliplification : public Runnable
{
public:
    FloatingPointMuliplification( ) : Runnable() { };
    virtual ~FloatingPointMuliplification() { };

    virtual void prepare(void) { };
    virtual void finalize(void) { };

    virtual void run(int loops)
    {
        _T temp = (_T)( 0 );
        _T val  = (_T)( 2 );

        for ( int i = 0; i < loops; ++i )
        {
            temp = val * val;
        }
    }
};

template <typename _T>
class CMathExponential : public Runnable
{
public:
    CMathExponential() { };
    ~CMathExponential() { };

    virtual void prepare(void) { };
    virtual void finalize(void) { };

    virtual void run( int loops )
    {
        _T temp = (_T)( 0 );
        const _T exponent = (_T)( 2 );

        for ( int i = 0; i < loops; ++i )
        {
            temp = exp( exponent );
        }
    }
};

template <typename _T>
class ArrayAccess : public Runnable
{
public:
    ArrayAccess(size_t size) : mSize(size), mArray(NULL) { };
    virtual ~ArrayAccess()
    {
        destroy();
        cout << "Destructor Get called." << endl;
    }

    virtual void prepare(void)
    {
        destroy();
        mArray = new _T[mSize];
    }

    virtual void finalize(void)
    {
        destroy();
    }

    virtual void run(int loops)
    {
        size_t p = 0;

        _T temp = (_T)( 0 );

        for ( int i = 0; i < loops; ++i )
        {
            temp = mArray[p];
            p++;

            if ( p == mSize )
            {
                p = 0;
            }
        }
    }

private:
    void destroy(void)
    {
        if ( NULL != mArray )
        {
            delete [] mArray; mArray = NULL;
        }
    }

private:
    const size_t mSize;
    _T* mArray;
};

class RunnableOperator
{
public:
    RunnableOperator() { };
    ~RunnableOperator() { };

    void profile( Runnable* rnb, int loops )
    {
        rnb->prepare();

        bpt::ptime start = bpt::microsec_clock::universal_time();

        rnb->run(loops);

        bpt::ptime end = bpt::microsec_clock::universal_time();
        bpt::time_duration td = end - start;

        rnb->finalize();

        cout << "Total time: " << td.total_milliseconds() << " ms." << endl;
    }
};

int main(void)
{
    cout << "Hello efficiency!" << endl;

    cout << "Show the maximum representable values of various types. " << endl;

    show_types();

    bml::unique_ptr<Runnable> rnbFloat(     new FloatingPointMuliplification<float> );
    bml::unique_ptr<Runnable> rnbDouble(    new FloatingPointMuliplification<double> );
    bml::unique_ptr<Runnable> rnbExpFloat(  new CMathExponential<float> );
    bml::unique_ptr<Runnable> rnbExpDouble( new CMathExponential<double> );
    bml::unique_ptr<Runnable> rnbAAFloat(   new ArrayAccess<float>(1000000) );
    bml::unique_ptr<Runnable> rnbAADouble(  new ArrayAccess<double>(1000000) );

    RunnableOperator ro;

    cout << "Executing time of various operations." << endl;

    ro.profile( rnbFloat.get(),     LOOP_NUM );
    ro.profile( rnbDouble.get(),    LOOP_NUM );
    ro.profile( rnbExpFloat.get(),  LOOP_NUM );
    ro.profile( rnbExpDouble.get(), LOOP_NUM );
    ro.profile( rnbAAFloat.get(),   LOOP_NUM );
    ro.profile( rnbAADouble.get(),  LOOP_NUM );

    // delete rnbAADouble; rnbAADouble = NULL;
    // delete rnbAAFloat; rnbAAFloat = NULL;
    // delete rnbExpDouble; rnbExpDouble = NULL;
    // delete rnbExpFloat; rnbExpFloat = NULL;
    // delete rnbDouble; rnbDouble = NULL;
    // delete rnbFloat; rnbFloat = NULL;

    return 0;
}


//[ Calc2
//  Copyright 2008 Eric Niebler. 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)
//
// This example enhances the simple arithmetic expression evaluator
// in calc1.cpp by using proto::extends to make arithmetic
// expressions immediately evaluable with operator (), a-la a
// function object

#include <iostream>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
namespace proto = boost::proto;
using proto::_;

template<typename Expr>
struct calculator_expression;

// Tell proto how to generate expressions in the calculator_domain
struct calculator_domain
  : proto::domain<proto::generator<calculator_expression> >
{};

// Will be used to define the placeholders _1 and _2
template<int I> struct placeholder {};

// Define a calculator context, for evaluating arithmetic expressions
// (This is as before, in calc1.cpp)
struct calculator_context
  : proto::callable_context< calculator_context const >
{
    // The values bound to the placeholders
    double d[2];

    // The result of evaluating arithmetic expressions
    typedef double result_type;

    explicit calculator_context(double d1 = 0., double d2 = 0.)
    {
        d[0] = d1;
        d[1] = d2;
    }

    // Handle the evaluation of the placeholder terminals
    template<int I>
    double operator ()(proto::tag::terminal, placeholder<I>) const
    {
        return d[ I - 1 ];
    }
};

// Wrap all calculator expressions in this type, which defines
// operator () to evaluate the expression.
template<typename Expr>
struct calculator_expression
  : proto::extends<Expr, calculator_expression<Expr>, calculator_domain>
{
    explicit calculator_expression(Expr const &expr = Expr())
      : calculator_expression::proto_extends(expr)
    {}

    BOOST_PROTO_EXTENDS_USING_ASSIGN(calculator_expression<Expr>)

    // Override operator () to evaluate the expression
    double operator ()() const
    {
        calculator_context const ctx;
        return proto::eval(*this, ctx);
    }

    double operator ()(double d1) const
    {
        calculator_context const ctx(d1);
        return proto::eval(*this, ctx);
    }

    double operator ()(double d1, double d2) const
    {
        calculator_context const ctx(d1, d2);
        return proto::eval(*this, ctx);
    }
};

// Define some placeholders (notice they're wrapped in calculator_expression<>)
calculator_expression<proto::terminal< placeholder< 1 > >::type> const _1;
calculator_expression<proto::terminal< placeholder< 2 > >::type> const _2;

// Now, our arithmetic expressions are immediately executable function objects:
int main()
{
    // Displays "5"
    std::cout << (_1 + 2.0)( 3.0 ) << std::endl;

    // Displays "6"
    std::cout << ( _1 * _2 )( 3.0, 2.0 ) << std::endl;

    // Displays "0.5"
    std::cout << ( (_1 - _2) / _2 )( 3.0, 2.0 ) << std::endl;

    return 0;
}
//]


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/eigen.h>

#include <Eigen/Core>
#include <optional>
#include <tuple>
#include <unordered_map>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/math.hpp"
#include "pyinterp/eigen.hpp"
#include "pyinterp/geodetic/box.hpp"
#include "pyinterp/geodetic/point.hpp"
#include "pyinterp/geodetic/polygon.hpp"

namespace pyinterp::geohash::int64 {

/// Returns the precision in longitude/latitude and degrees for the given
/// precision
[[nodiscard]] inline auto error_with_precision(const uint32_t precision)
    -> std::tuple<double, double> {
  auto lat_bits = static_cast<int32_t>(precision >> 1U);
  auto lng_bits = static_cast<int32_t>(precision - lat_bits);

  return std::make_tuple(360 * detail::math::power2(-lng_bits),
                         180 * detail::math::power2(-lat_bits));
}

// Encode a point into geohash with the given precision
[[nodiscard]] auto encode(const geodetic::Point& point, uint32_t precision)
    -> uint64_t;

// Encode points into geohash with the given precision
[[nodiscard]] inline auto encode(const Eigen::Ref<const Eigen::VectorXd>& lon,
                                 const Eigen::Ref<const Eigen::VectorXd>& lat,
                                 uint32_t precision) -> Vector<uint64_t> {
  detail::check_eigen_shape("lon", lon, "lat", lat);
  auto size = lon.size();
  auto result = Vector<uint64_t>(size);
  for (Eigen::Index ix = 0; ix < size; ++ix) {
    result(ix) = encode({lon[ix], lat[ix]}, precision);
  }
  return result;
}

// Returns the region encoded by the integer geohash with the specified
// precision.
[[nodiscard]] auto bounding_box(uint64_t hash, uint32_t precision)
    -> geodetic::Box;

// Decode a hash into a geographic point with the given precision.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(const uint64_t hash, const uint32_t precision,
                                 const bool round) -> geodetic::Point {
  auto bbox = bounding_box(hash, precision);
  return round ? bbox.round() : bbox.centroid();
}

// Decode hashes into a geographic points with the given bit depth.
// If round is true, the coordinates of the points will be rounded to the
// accuracy defined by the GeoHash.
[[nodiscard]] inline auto decode(const Eigen::Ref<const Vector<uint64_t>>& hash,
                                 const uint32_t precision, const bool center)
    -> std::tuple<Eigen::VectorXd, Eigen::VectorXd> {
  auto lon = Eigen::VectorXd(hash.size());
  auto lat = Eigen::VectorXd(hash.size());
  auto point = geodetic::Point();
  for (Eigen::Index ix = 0; ix < hash.size(); ++ix) {
    point = decode(hash(ix), precision, center);
    lon[ix] = point.lon();
    lat[ix] = point.lat();
  }
  return std::make_tuple(lon, lat);
}

// Returns all neighbors hash clockwise from north around northwest at the given
// precision.
// 7 0 1
// 6 x 2
// 5 4 3
[[nodiscard]] auto neighbors(uint64_t hash, uint32_t precision)
    -> Eigen::Matrix<uint64_t, 8, 1>;

// Returns the property of the grid covering the given box: geohash of the
// minimum corner point, number of boxes in longitudes and latitudes.
[[nodiscard]] auto grid_properties(const geodetic::Box& box, uint32_t precision)
    -> std::tuple<uint64_t, size_t, size_t>;

// Returns all the GeoHash codes within the box.
[[nodiscard]] auto bounding_boxes(const std::optional<geodetic::Box>& box,
                                  uint32_t precision) -> Vector<uint64_t>;

// Returns all the GeoHash codes within the polygon.
[[nodiscard]] inline auto bounding_boxes(const geodetic::Polygon& polygon,
                                         uint32_t chars) -> Vector<uint64_t> {
  auto box = geodetic::Box();
  boost::geometry::envelope<geodetic::Polygon, geodetic::Box>(polygon, box);
  return bounding_boxes(box, chars);
}

// Returns the area covered by the GeoHash
[[nodiscard]] inline auto area(uint64_t hash, uint32_t precision,
                               const std::optional<geodetic::System>& wgs)
    -> double {
  return bounding_box(hash, precision).area(wgs);
}

// Returns the start and end indexes of the different GeoHash boxes.
[[nodiscard]] auto where(
    const pybind11::EigenDRef<const Eigen::Matrix<uint64_t, -1, -1>>& hash)
    -> std::unordered_map<uint64_t, std::tuple<std::tuple<int64_t, int64_t>,
                                     std::tuple<int64_t, int64_t>>>;

}  // namespace pyinterp::geohash::int64


/*
 * dynamical-system.hpp
 *
 *  Created on: 19 mai 2014
 *      Author: alexis
 */

#ifndef DYNAMICAL_SYSTEM_HPP_
#define DYNAMICAL_SYSTEM_HPP_

#include <vector>

#include <state-observation/api.h>
#include <state-observation/dynamical-system/dynamical-system-functor-base.hpp>
#include <state-observation/noise/noise-base.hpp>
#include <state-observation/sensors-simulation/accelerometer-gyrometer.hpp>
#include <state-observation/tools/hrp2.hpp>
#include <state-observation/tools/rigid-body-kinematics.hpp>

#include <Eigen/Cholesky>

namespace stateObservation
{

namespace flexibilityEstimation
{

/**
 * \class  DynamicalSystem
 * \brief  This class describes the dynamics of a robot's flexibility
 *         this dynamics is the simplest possible system, the flexibility
 *         is expressed as a rotation against the contact positions with no
 *         other hypothesis than that the contact points are at constant position
 *
 */
class STATE_OBSERVATION_DLLAPI IMUElasticLocalFrameDynamicalSystem : public stateObservation::DynamicalSystemFunctorBase
{
public:
  struct input
  {
    /// indices of the different components of a vector of the input state
    static constexpr unsigned posCom = 0;
    static constexpr unsigned velCom = 3;
    static constexpr unsigned accCom = 6;
    static constexpr unsigned inertia = 9;
    static constexpr unsigned angMoment = 15;
    static constexpr unsigned dotInertia = 18;
    static constexpr unsigned dotAngMoment = 24;
    static constexpr unsigned posIMU = 27;
    static constexpr unsigned oriIMU = 30;
    static constexpr unsigned linVelIMU = 33;
    static constexpr unsigned angVelIMU = 36;
    static constexpr unsigned linAccIMU = 39;
    static constexpr unsigned additionalForces = 42;
    static constexpr unsigned contacts = 48;

    static constexpr unsigned sizeBase = 48;
  };

  struct state
  {
    static constexpr unsigned pos = 0;
    static constexpr unsigned ori = 3;
    static constexpr unsigned linVel = 6;
    static constexpr unsigned angVel = 9;
    static constexpr unsigned fc = 12;
    static constexpr unsigned unmodeledForces = 24;
    static constexpr unsigned comBias = 30;
    static constexpr unsigned drift = 32;

    static constexpr unsigned size = 35;
  };

  struct contactModel
  {
    /// indices of the different components of a vector of the input state
    static constexpr unsigned elasticContact = 1;
    static constexpr unsigned pendulum = 2;
    static constexpr unsigned none = 0;
  };

  typedef Eigen::LLT<Matrix3> LLTMatrix3;

  /// constructor
  explicit IMUElasticLocalFrameDynamicalSystem(double dt);

  /// virtual destructor
  virtual ~IMUElasticLocalFrameDynamicalSystem();

  void test();

  // Get the contact wrench
  void computeContactWrench(const Matrix3 & orientation,
                            const Vector3 & position,
                            const IndexedVectorArray & contactPosV,
                            const IndexedVectorArray & contactOriV,
                            const Vector & fc,
                            const Vector & tc,
                            const Vector3 & fm,
                            const Vector3 & tm,
                            const Vector3 & addForce,
                            const Vector3 & addMoment);

  stateObservation::Vector computeAccelerations(const Vector & x, const Vector & u);

  // computation of the acceleration linear
  virtual void computeAccelerations(const Vector3 & positionCom,
                                    const Vector3 & velocityCom,
                                    const Vector3 & accelerationCom,
                                    const Vector3 & AngMomentum,
                                    const Vector3 & dotAngMomentum,
                                    const Matrix3 & Inertia,
                                    const Matrix3 & dotInertia,
                                    const IndexedVectorArray & contactPos,
                                    const IndexedVectorArray & contactOri,
                                    const Vector3 & position,
                                    const Vector3 & linVelocity,
                                    Vector3 & linearAcceleration,
                                    const Vector3 & oriVector,
                                    const Matrix3 & orientation,
                                    const Vector3 & angularVel,
                                    Vector3 & angularAcceleration,
                                    const Vector & fc,
                                    const Vector & tc,
                                    const Vector3 & fm,
                                    const Vector3 & tm,
                                    const Vector3 & addForces,
                                    const Vector3 & addMoments);

  /// Description of the state dynamics
  virtual stateObservation::Vector stateDynamics(const stateObservation::Vector & x,
                                                 const stateObservation::Vector & u,
                                                 TimeIndex k);

  /// compute the jacobien of the state dynamics at the last computed value
  stateObservation::Matrix stateDynamicsJacobian();

  /// compute the jacobien of the state dynamics at a given state
  stateObservation::Matrix stateDynamicsJacobian(const stateObservation::Vector & x,
                                                 const stateObservation::Vector & u,
                                                 TimeIndex k);

  /// sets the finite differences derivation step vector
  void setFDstep(const stateObservation::Vector & dx);

  /// Description of the sensor's dynamics
  virtual stateObservation::Vector measureDynamics(const stateObservation::Vector & x,
                                                   const stateObservation::Vector & u,
                                                   TimeIndex k);

  /// compute the Jacobien of the measurements dynamics at the last computed value
  stateObservation::Matrix measureDynamicsJacobian();

  /// compute the Jacobien of the measurements dynamics at a given state value
  stateObservation::Matrix measureDynamicsJacobian(const stateObservation::Vector & x,
                                                   const stateObservation::Vector & u,
                                                   TimeIndex k);

  /// Sets a noise which disturbs the state dynamics
  virtual void setProcessNoise(stateObservation::NoiseBase *);

  /// Removes the process noise
  virtual void resetProcessNoise();

  /// Gets the process noise
  virtual stateObservation::NoiseBase * getProcessNoise() const;

  /// Sets a noise which disturbs the measurements
  virtual void setMeasurementNoise(stateObservation::NoiseBase *);

  /// Removes the measurement noise
  virtual void resetMeasurementNoise();

  /// Gets a pointer on the measurement noise
  virtual stateObservation::NoiseBase * getMeasurementNoise() const;

  /// Set the period of the time discretization
  virtual void setSamplingPeriod(double dt);

  /// Gets the state size
  virtual Index getStateSize() const;

  /// Gets the input size
  virtual Index getInputSize() const;

  /// Sets the input size
  virtual void setInputSize(Index i);

  /// Gets the contact number
  /// virtual

  /// Gets the contacts position

  /// Gets the measurement size
  virtual Index getMeasurementSize() const;

  /// Sets the number of contacts
  virtual void setContactsNumber(unsigned);

  virtual void setPe(stateObservation::Vector3 Pe)
  {
    pe = Pe;
  }

  /// Gets the nimber of contacts
  inline unsigned getContactsNumber(void) const
  {
    return nbContacts_;
  }

  virtual void setContactModel(unsigned nb);

  virtual void setPrinted(bool b)
  {
    printed_ = b;
  }

  virtual void computeElastContactForcesAndMoments(const IndexedVectorArray & contactPosArray,
                                                   const IndexedVectorArray & contactOriArray,
                                                   const IndexedVectorArray & contactVelArray,
                                                   const IndexedVectorArray & contactAngVelArray,
                                                   const Vector3 & position,
                                                   const Vector3 & linVelocity,
                                                   const Vector3 & oriVector,
                                                   const Matrix3 & orientation,
                                                   const Vector3 & angVel,
                                                   Vector & fc,
                                                   Vector & tc);

  virtual void computeElastPendulumForcesAndMoments(const IndexedVectorArray & PrArray,
                                                    const Vector3 & position,
                                                    const Vector3 & linVelocity,
                                                    const Vector3 & oriVector,
                                                    const Matrix3 & orientation,
                                                    const Vector3 & angVel,
                                                    Vector & forces,
                                                    Vector & moments);

  void computeForcesAndMoments(const IndexedVectorArray & position1,
                               const IndexedVectorArray & position2,
                               const IndexedVectorArray & velocity1,
                               const IndexedVectorArray & velocity2,
                               const Vector3 & position,
                               const Vector3 & linVelocity,
                               const Vector3 & oriVector,
                               const Matrix3 & orientation,
                               const Vector3 & angVel,
                               Vector & fc,
                               Vector & tc);

  virtual void computeForcesAndMoments(const Vector & x, const Vector & u);

  virtual Vector getForcesAndMoments();

  virtual Vector getForcesAndMoments(const Vector & x, const Vector & u);

  virtual Vector getMomentaDotFromForces(const Vector & x, const Vector & u);
  virtual Vector getMomentaDotFromKinematics(const Vector & x, const Vector & u);

  virtual void iterateDynamicsEuler(const Vector3 & positionCom,
                                    const Vector3 & velocityCom,
                                    const Vector3 & accelerationCom,
                                    const Vector3 & AngMomentum,
                                    const Vector3 & dotAngMomentum,
                                    const Matrix3 & Inertia,
                                    const Matrix3 & dotInertia,
                                    const IndexedVectorArray & contactPos,
                                    const IndexedVectorArray & contactOri,
                                    Vector3 & position,
                                    Vector3 & linVelocity,
                                    Vector & fc1,
                                    Vector3 & oriVector,
                                    Vector3 & angularVel,
                                    Vector & fc2,
                                    const Vector3 & fm,
                                    const Vector3 & tm,
                                    const Vector3 & addForces,
                                    const Vector3 & addMoments,
                                    double dt);

  virtual void iterateDynamicsRK4(const Vector3 & positionCom,
                                  const Vector3 & velocityCom,
                                  const Vector3 & accelerationCom,
                                  const Vector3 & AngMomentum,
                                  const Vector3 & dotAngMomentum,
                                  const Matrix3 & Inertia,
                                  const Matrix3 & dotInertia,
                                  const IndexedVectorArray & contactPos,
                                  const IndexedVectorArray & contactOri,
                                  Vector3 & position,
                                  Vector3 & linVelocity,
                                  Vector & fc1,
                                  Vector3 & oriVector,
                                  Vector3 & angularVel,
                                  Vector & fc2,
                                  const Vector3 & fm,
                                  const Vector3 & tm,
                                  const Vector3 & addForces,
                                  const Vector3 & addMoments,
                                  double dt);

  virtual void setWithForceMeasurements(bool b);
  virtual bool getWithForceMeasurements() const;
  virtual void setWithComBias(bool b);
  virtual bool getWithComBias() const;
  virtual void setWithAbsolutePosition(bool b);
  virtual bool getWithAbsolutePosition() const;
  void setWithUnmodeledForces(bool b);

  virtual void setKfe(const Matrix3 & m);
  virtual void setKfv(const Matrix3 & m);
  virtual void setKte(const Matrix3 & m);
  virtual void setKtv(const Matrix3 & m);

  virtual void setKfeRopes(const Matrix3 & m);
  virtual void setKfvRopes(const Matrix3 & m);
  virtual void setKteRopes(const Matrix3 & m);
  virtual void setKtvRopes(const Matrix3 & m);

  virtual Matrix getKfe() const;
  virtual Matrix getKfv() const;
  virtual Matrix getKte() const;
  virtual Matrix getKtv() const;

  virtual void setRobotMass(double d);

  virtual double getRobotMass() const;

protected:
  bool printed_;

  stateObservation::AccelerometerGyrometer sensor_;

  stateObservation::NoiseBase * processNoise_;

  void updateMeasurementSize_();

  double dt_;

  double robotMass_;
  double robotMassInv_;

  Matrix3 & computeRotation_(const Vector3 & x, int i);

  static constexpr Index stateSize_ = state::size;
  Index inputSize_;
  static constexpr Index measurementSizeBase_ = 6;
  unsigned nbContacts_;
  unsigned contactModel_;

  Vector fc_;
  Vector tc_;

  Vector dx_;

  Vector xk1_;
  Vector xk_;
  Vector uk_;

  Vector xk_fory_;
  Vector yk_;
  Vector uk_fory_;

  Index measurementSize_;

  std::vector<Vector3, Eigen::aligned_allocator<Vector3>> contactPositions_;

  Matrix3 Kfe_, Kte_, Kfv_, Ktv_;
  Matrix3 KfeRopes_, KteRopes_, KfvRopes_, KtvRopes_;

  TimeIndex kcurrent_;

  bool withForceMeasurements_;
  bool withComBias_;
  bool withAbsolutePos_;
  bool withUnmodeledForces_;

  stateObservation::Vector3 pe;

  double marginalStabilityFactor_;
  // a scaling factor a=1-epsilon to avoid the natural marginal stability of
  // the dynamics x_{k+1}=x_k we replace it with x_{k+1}=a*x_k

  unsigned index_;

  struct Optimization
  {

    Vector6 momentaDot;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector3 positionFlex;
    Vector3 velocityFlex;
    Vector3 accelerationFlex;
    Vector3 orientationFlexV;
    Vector3 angularVelocityFlex;
    Vector3 angularAccelerationFlex;
    Vector3 positionComBias;

    Matrix3 rFlex;
    Matrix3 rFlexT;

    Vector3 drift;
    Vector3 pdrift;
    Matrix3 rdrift;

    double cy, sy;

    AngleAxis orientationAA;

    Vector xk1;
    Vector xk;
    Vector xk1dx;

    Vector xdx;

    Vector xk_fory;
    Vector yk;
    Vector ykdy;

    TimeIndex k_fory;

    Matrix3 orinertia;

    LLTMatrix3 invinertia;

    Matrix3 rtotal;
    Vector3 ptotal;
    AngleAxis aatotal;
    Vector3 oritotal;

    Matrix Jx;
    Matrix Jy;

    Matrix3 rimu;
    Vector3 imuAcc;
    Vector3 imuOmega;
    Vector sensorState;

    Vector3 positionCom;
    Vector3 velocityCom;
    Vector3 accelerationCom;
    Vector3 AngMomentum;
    Vector3 dotAngMomentum;

    Vector3 positionControl;
    Vector3 velocityControl;
    Vector3 accelerationControl;
    Vector3 orientationControlV;
    Vector3 angularVelocityControl;

    Matrix3 rControl;

    IndexedVectorArray contactPosV;
    IndexedVectorArray contactOriV;
    IndexedVectorArray contactVelArray;
    IndexedVectorArray contactAngVelArray;

    Matrix3 inertia;
    Matrix3 dotInertia;

    IndexedVectorArray efforts;

    Vector3 f, fi;
    Vector3 t;

    // unmodelled and unmeasured forces
    Vector3 fm;
    Vector3 tm;

    Vector3 linearAcceleration;
    Vector3 angularAcceleration;

    Vector3 vf;
    Vector3 vt;

    Vector3 crosstempV;
    Matrix3 crosstempM;

    // elastic contact forces and moments
    Matrix3 Rci; // rotation of contact i
    Matrix3 Rcit; // transpose of previous
    Vector3 contactPos; //
    Vector3 contactVel;
    Vector3 RciContactPos;
    Vector3 globalContactPos;
    Matrix3 Rt;

    Vector3 forcei;
    Vector3 momenti;

    // additional forces and moments
    Vector3 addForce;
    Vector3 addMoment;

    Matrix3 skewV;
    Matrix3 skewV2;
    Matrix3 skewVR;
    Matrix3 skewV2R;
    Matrix3 RIRT;
    Vector3 wx2Rc;
    Vector3 _2wxRv;
    Vector3 Ra;
    Vector3 Rc;
    Vector3 Rcp;

    // optimization of orientation transformation between vector3 to rotation matrix

    Matrix3 curRotation0;
    Vector3 orientationVector0;
    Matrix3 curRotation1;
    Vector3 orientationVector1;
    Matrix3 curRotation2;
    Vector3 orientationVector2;
    Matrix3 curRotation3;
    Vector3 orientationVector3;

    Optimization()
    : curRotation0(Matrix3::Identity()), orientationVector0(Vector3::Zero()), curRotation1(Matrix3::Identity()),
      orientationVector1(Vector3::Zero()), curRotation2(Matrix3::Identity()), orientationVector2(Vector3::Zero()),
      curRotation3(Matrix3::Identity()), orientationVector3(Vector3::Zero())
    {
    }

    inline Vector3 & orientationVector(int i)
    {
      if(i == 0) return orientationVector0;
      if(i == 1) return orientationVector1;
      if(i == 2) return orientationVector2;

      return orientationVector3;
    }

    inline Matrix3 & curRotation(int i)
    {
      if(i == 0) return curRotation0;
      if(i == 1) return curRotation1;
      if(i == 2) return curRotation2;

      return curRotation3;
    }

  } op_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace flexibilityEstimation
} // namespace stateObservation

#endif /* DYNAMICAL_SYSTEM_HPP_ */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/oneotwoeps.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/log1p.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( acosh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        A0 t = dec(a0);
        auto test = is_greater(t,Oneotwoeps<A0>());
        A0 z = if_else(test, a0, t+sqrt(t+t+sqr(t)));
        return if_plus(test, log1p(z), Log_2<A0>());
      }
   };

} } }

#endif


//          Copyright surrealwaffle 2018 - 2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstddef>
#include <cmath>

#include <algorithm>
#include <functional>
#include <type_traits>
#include <utility>

#include <sentinel/config.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/register/point.hpp>

namespace sentinel { namespace math {

template<class T, std::size_t N>
struct vector {
    using value_type      = T;
    using size_type       = std::size_t;
    using difference_type = std::ptrdiff_t;
    using reference       = value_type&;
    using const_reference = const value_type&;
    using pointer         = value_type*;
    using const_pointer   = const value_type*;
    using iterator        = pointer;
    using const_iterator  = const_pointer;

    static_assert(N > 0);
    value_type array[N];

    static inline constexpr vector zero = vector(/*ZERO INITIALIZED*/);

    constexpr reference       operator[](size_type pos)       { return array[pos]; }
    constexpr const_reference operator[](size_type pos) const { return array[pos]; }

    constexpr pointer       data() noexcept       { return array; }
    constexpr const_pointer data() const noexcept { return array; }

    static
    constexpr size_type size() noexcept { return N; }

    constexpr iterator begin() noexcept { return array; }
    constexpr iterator end()   noexcept { return begin() + size(); }

    constexpr const_iterator cbegin() const noexcept { return array; }
    constexpr const_iterator cend()   const noexcept { return begin() + size(); }

    constexpr const_iterator begin() const noexcept { return cbegin(); }
    constexpr const_iterator end()   const noexcept { return cend(); }

    constexpr void fill(const T& value) { for (auto& v : *this) v = value; }

    constexpr bool operator==(const vector& other) const {
        return std::mismatch(begin(), end(), other.begin()).first == end();
    }

    constexpr bool operator!=(const vector& other) const {
        return !(*this == other);
    }

    static constexpr vector filled(const T& value)
    {
        auto return_fill_value = [&value] (auto&&...) -> const T& { return value; };
        return [&return_fill_value] <std::size_t... I> (std::index_sequence<I...>) {
            return vector{return_fill_value(I)...};
        } (std::make_index_sequence<N>{});
    }

    template<class BinaryOp>
    constexpr vector& pointwise_transform(const vector& other, BinaryOp op)
    {
        auto other_it = other.begin();
        for (auto it = begin(), end = this->end(); it != end; ++it, ++other_it)
             *it = op(*it, *other_it);
        return *this;
    }

    constexpr vector& operator+=(const vector& other) { return pointwise_transform(other, std::plus{}); }
    constexpr vector& operator-=(const vector& other) { return pointwise_transform(other, std::minus{}); }
    constexpr vector& operator*=(const vector& other) { return pointwise_transform(other, std::multiplies{}); }
    constexpr vector& operator/=(const vector& other) { return pointwise_transform(other, std::divides{}); }

    constexpr vector& operator*=(const T& scalar) { return (*this) *= filled(scalar); }

    constexpr vector operator+(const vector& other) const { vector result = *this; result += other; return result; }
    constexpr vector operator-(const vector& other) const { vector result = *this; result -= other; return result; }
    constexpr vector operator*(const vector& other) const { vector result = *this; result *= other; return result; }
    constexpr vector operator/(const vector& other) const { vector result = *this; result /= other; return result; }

    constexpr vector operator-() const;
};

template<class T, std::size_t N>
constexpr vector<T, N> vector<T, N>::operator-() const
{
    vector res = *this;
    for (auto& v : res)
        v = -v;
    return res;
}

/** \brief Computes the dot product of two vectors.
 */
template<class T, std::size_t N>
constexpr T dot(const vector<T, N>& a, const vector<T, N>& b)
{
    T result = static_cast<T>(0);
    for (std::size_t pos = 0; pos < N; ++pos)
        result += a[pos] * b[pos];
    return result;
}

/** \brief Computes the cross product of two vector.
 */
template<class T>
constexpr vector<T, 3> cross(const vector<T, 3>& a, const vector<T, 3>& b)
{
    return {
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0]
    };
}

/** \brief Computes the square of the norm of a vector.
 */
template<class T, std::size_t N>
constexpr float norm2(const vector<T, N>& vec) { return dot(vec, vec); }

/** \brief Computes the norm of a vector.
 */
template<class T, std::size_t N>
constexpr float norm(const vector<T, N>& vec) { return std::sqrt(norm2(vec)); }

/** \brief Computes a normalized vector.
 *
 * If the norm of \a vec is less than or equal to SENTINEL_VECTOR_SMALL_NORM,
 * the returned vector is the `0` vector.
 */
template<class T, std::size_t N>
constexpr vector<T, N> normalized(const vector<T, N>& vec)
{
    const auto length = norm(vec);
    return length > SENTINEL_VECTOR_SMALL_NORM ? (1 / length) * vec
                                               : vector<T, N>::zero;
}

/** \brief Performs scalar-vector multiplication.
 */
template<class T, std::size_t N>
constexpr vector<T, N> operator*(const T& scalar, const vector<T, N>& vec)
{
    return vector<T, N>::filled(scalar) * vec;
}

/** \brief Performs scalar-vector multiplication.
 */
template<class T, std::size_t N>
constexpr vector<T, N> operator*(const vector<T, N>& vec, const T& scalar)
{
    return vec * vector<T, N>::filled(scalar);
}

template<class T, std::size_t N, class BinaryOp>
constexpr vector<T, N> pointwise_compose(const vector<T, N>& a,
                                         const vector<T, N>& b,
                                         BinaryOp op)
{
    vector<T, N> result = a;
    a.pointwise_accumulate(a, std::cref(op));
    return result;
}

using fvec2 = vector<float, 2>;
using fvec3 = vector<float, 3>;

using dvec2 = vector<double, 2>;
using dvec3 = vector<double, 3>;

} } // namespace sentinel::math

BOOST_GEOMETRY_REGISTER_POINT_2D(sentinel::math::fvec2,
                                 float,
                                 boost::geometry::cs::cartesian,
                                 operator[](0),
                                 operator[](1)
                                 );

BOOST_GEOMETRY_REGISTER_POINT_3D(sentinel::math::fvec3,
                                 float,
                                 boost::geometry::cs::cartesian,
                                 operator[](0),
                                 operator[](1),
                                 operator[](2)
                                 );

BOOST_GEOMETRY_REGISTER_POINT_2D(sentinel::math::dvec2,
                                 double,
                                 boost::geometry::cs::cartesian,
                                 operator[](0),
                                 operator[](1)
                                 );

BOOST_GEOMETRY_REGISTER_POINT_3D(sentinel::math::dvec3,
                                 double,
                                 boost::geometry::cs::cartesian,
                                 operator[](0),
                                 operator[](1),
                                 operator[](2)
                                 );


#include <benchmark/benchmark.h>
#include <random>
#include <complex>
#include <boost/math/fft/bsl_backend.hpp>
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/math/constants/constants.hpp>

#include "fft_test_helpers.hpp"

std::default_random_engine gen;
std::uniform_real_distribution<double> distribution;

typedef std::complex<double> cd;
std::vector<cd> random_vec(size_t N)
{
    std::vector<cd> V(N);
    for (auto& x : V)
      x = distribution(gen);
    return V;
}

void bench_bsl(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using fft_transform = boost::math::fft::bsl_transform;
        fft_transform::forward(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_gsl(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using fft_transform = boost::math::fft::gsl_transform;
        fft_transform::forward(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_fftw(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using fft_transform = boost::math::fft::fftw_transform;
        fft_transform::forward(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}

//// powers of 2
//BENCHMARK(bench_bsl)
//    ->RangeMultiplier(4)
//    ->Range(1 << 8, 1 << 20)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_gsl)
//    ->RangeMultiplier(4)
//    ->Range(1 << 8, 1 << 20)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_fftw)
//    ->RangeMultiplier(4)
//    ->Range(1 << 8, 1 << 20)
//    ->Complexity(benchmark::oNLogN);

// powers of 10
//BENCHMARK(bench_bsl)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_gsl)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);
//
//BENCHMARK(bench_fftw)
//    ->RangeMultiplier(10)
//    ->Range(100, 1000000)
//    ->Complexity(benchmark::oNLogN);

// primes
BENCHMARK(bench_bsl)
    ->Arg(109)
    ->Arg(1009)
    ->Arg(10009)
    ->Arg(100003)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_gsl)
    ->Arg(109)
    ->Arg(1009)
    ->Arg(10009)
    ->Arg(100003)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_fftw)
    ->Arg(109)
    ->Arg(1009)
    ->Arg(10009)
    ->Arg(100003)
    ->Complexity(benchmark::oNLogN);

BENCHMARK_MAIN();


/** @file
 * @brief NPDE WaveABC2D
 * @author Erick Schulz
 * @date 11/12/2019
 * @copyright Developed at ETH Zurich
 */

#include "waveabc2d.h"

#include <iostream>
#include <memory>
#include <string>
// Eigen includes
#include <Eigen/Core>
// Lehrfem++ includes
#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

using namespace WaveABC2D;

int main(int /*argc*/, const char ** /*argv*/) {
  std::cout << "\n" << std::endl;
  std::cout << "PROBLEM - WaveABC2D" << std::endl;

  testConvergenceScalarImplicitTimestepping();

  // Load mesh into a Lehrfem++ object
  std::cout << "Loading mesh..." << std::endl;
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../meshes/unitsquare2.msh");
  auto mesh_p = reader.mesh();  // type shared_ptr< const lf::mesh::Mesh>

  // Finite element space
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

  auto mu0 = [](const Eigen::Vector2d &x) -> double {
    return std::sin(x.norm());
  };
  auto nu0 = [](const Eigen::Vector2d &x) -> double { return std::cos(x(1)); };
  auto rho = [](Eigen::Vector2d) -> double { return 1.0; };

  WaveABC2DTimestepper<decltype(rho), decltype(mu0), decltype(nu0)> stepper(
      fe_space_p, rho, 250, 1.0);
  Eigen::VectorXd discrete_solution = stepper.solveWaveABC2D(mu0, nu0);

  double discrete_energy = stepper.energies();

  // Output results to vtk file
  lf::io::VtkWriter vtk_writer(mesh_p, "WaveABC2D_solution.vtk");
  // Write nodal data taking the values of the discrete solution at the vertices
  auto nodal_data = lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 2);
  for (int global_idx = 0; global_idx < N_dofs; global_idx++) {
    nodal_data->operator()(dofh.Entity(global_idx)) =
        discrete_solution[global_idx];
  };
  vtk_writer.WritePointData("WaveABC2D_solution", *nodal_data);

  std::cout << "\nThe WaveABC2D_solution was written to:" << std::endl;
  std::cout << "WaveABC2D_solution.vtk\n" << std::endl;

  std::cout << "The discrete energies E^(k) : " << discrete_energy << std::endl;
  return 0;
}  // main


//==============================================================================
//         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 polynom toolbox - roots/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/roots.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/isulpequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( roots_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::roots;
  using nt2::tag::roots_;
  typedef std::complex<T> cT;
  nt2::table<T> p =  nt2::_(T(1), T(3));
  p(2) = T(-3); p(3) = T(2);
  nt2::table<T> c = nt2::_(T(2), T(-1), T(1));
  NT2_DISPLAY(roots(p));
  NT2_DISPLAY(c);
  NT2_TEST(nt2::isulpequal(nt2::real(roots(p)), c, T(5.0)));
} // end of test for floating_


#include <bitset>
#include <vector>
//#include "stdint.h"
#include <iostream>
#include <functional>
#include <ctime>
#include <chrono>
#include <sys/time.h>
#include <boost/dynamic_bitset.hpp>
using namespace boost;

struct timeval start, end;

void startTimer()
{
  gettimeofday(&start,NULL);
}
void stopTimer()
{
  gettimeofday(&end,NULL);
}
int getMs()
{
  return  (end.tv_sec - start.tv_sec)*1000 + (end.tv_usec-start.tv_usec)/1000;
}
int getUs()
{
  return  (end.tv_sec - start.tv_sec)*1000*1000 + (end.tv_usec-start.tv_usec);
}

int main()
{
  // empty constructor
  const int NUM = 10000000;
  std::vector<uint64_t>  bitwise_v;
  std::vector<std::bitset<64> >  bitset_v;
  std::vector<boost::dynamic_bitset<> > boost_bitset_v;

  for (int i = 0; i < NUM; ++i)
  {
    std::bitset<64> v(i);
    boost::dynamic_bitset<> dv(64, i);
    bitset_v.push_back(v);
    bitwise_v.push_back(v.to_ulong());
    boost_bitset_v.push_back(dv);
  }

  uint64_t biter = 100;
  std::bitset<64> bitseter(100);
  boost::dynamic_bitset<> boost_bitsetr(64, 100);
  int wise_count = 0;
  int bitset_count = 0;
  int boost_count = 0;
  startTimer();
  for (std::vector<uint64_t>::const_iterator wisei = bitwise_v.begin(); wisei != bitwise_v.end(); ++wisei) {
    if ((biter & *wisei ) == biter)
    {
      wise_count++;
    }
  }
  stopTimer();
  std::cout << "time cost Ms: " << getMs() << std::endl;
  std::cout << "time cost Us: " << getUs() << std::endl;
  std::cout << "wise_count: " << wise_count << std::endl;

  startTimer();
  for (std::vector<std::bitset<64> >::const_iterator seti = bitset_v.begin(); seti != bitset_v.end(); ++seti) {
    if ((bitseter & *seti) == bitseter)
    {
      bitset_count++;
    }
  }
  stopTimer();
  std::cout << "time cost Ms: " << getMs() << std::endl;
  std::cout << "time cost Us: " << getUs() << std::endl;
  std::cout << "biter_count: " << bitset_count << std::endl;

// boost dynamic bitset
  startTimer();
  for (std::vector<boost::dynamic_bitset<> >::const_iterator dbi = boost_bitset_v.begin(); dbi != boost_bitset_v.end(); ++dbi) {
    if ((boost_bitsetr & *dbi) == boost_bitsetr)
    {
      boost_count++;
    }
  }
  stopTimer();
  std::cout << "time cost Ms: " << getMs() << std::endl;
  std::cout << "time cost Us: " << getUs() << std::endl;
  std::cout << "boost_count: " << boost_count << std::endl;
  // unsigned long long constructor
  //std::bitset<8> b02(42);          // [0,0,1,0,1,0,1,0]
  //std::bitset<70> bl(ULLONG_MAX); // [0,0,0,0,0,0,1,1,1,...,1,1,1] in C++11
  //std::bitset<8> bs(0xfff0);      // [1,1,1,1,0,0,0,0]

  //// string constructor
  //std::string bit_string = "110010";
  //std::bitset<8> b3(bit_string); // [0,0,1,1,0,0,1,0]
  //std::bitset<8> b4(bit_string, 2); // [0,0,0,0,0,0,1,0]
  //std::bitset<8> b5(bit_string, 2, 3); // [0,0,0,0,0,0,0,1]

  //// string constructor using custom zero/one digits
  //std::string alpha_bit_string = "aBaaBBaB";
  //std::bitset<8> b6 (alpha_bit_string, 0, alpha_bit_string.size(), 'a', 'B'); // [0,1,0,0,1,1,0,1]

  //// char* constructor using custom digits
  //std::bitset<8> b7("XXXXYYYY", 8, 'X', 'Y');

  //std::cout << b01 << '\n' << b02 << '\n' << bl << '\n' << bs << '\n'
  //<< b3 << '\n' << b4 << '\n' << b5 << '\n' << b6 << '\n'
  //<< b7 << '\n';

  //// bitwise operation
  //std::bitset<4> b1("0110");
  //std::bitset<4> b2("0011");
  //std::cout << "b1 & b2: " << (b1 & b2) << '\n';
  //std::cout << "b1 | b2: " << (b1 | b2) << '\n';
  //std::cout << "b1 ^ b2: " << (b1 ^ b2) << '\n';

  //// operation all any none
  //std::bitset<4> bo1("0000");
  //std::bitset<4> bo2("0101");
  //std::bitset<4> bo3("1111");

  //std::cout << "bitset\t" << "all\t" << "any\t" << "none\n";
  //std::cout << bo1 << '\t' << bo1.all() << '\t' << bo1.any() << '\t' << bo1.none() << '\n';
  //std::cout << bo2 << '\t' << bo2.all() << '\t' << bo2.any() << '\t' << bo2.none() << '\n';
  //std::cout << bo3 << '\t' << bo3.all() << '\t' << bo3.any() << '\t' << bo3.none() << '\n';
}


/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Based on the SMARTIES MATLAB package by W.R.C. Somerville, B. Auguié, E.C. Le Ru
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

Various small maths functions, helper functions and others used throughout SMARTIES functions that don't fit anywhere.
*/

#ifndef SMARTIES_MATH_HPP
#define SMARTIES_MATH_HPP

#include "core.hpp"
#include <Eigen/LU>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/constants/constants.hpp>

using namespace Eigen;
using namespace std;

namespace Smarties {
  /* Returns the mathematical constant pi with the amount of precision specified by template argument */
  template <class Real>
  inline Real mp_pi(void) { return boost::math::constants::pi<Real>(); }

  /* Returns the machine epsilon of the type given in the template argument */
  template <class Real>
  inline Real mp_eps(void) { return numeric_limits<Real>::epsilon(); }

  /* Returns the imaginary unit 'i' with the type given in the template argument */
  template <class Real>
  inline complex<Real> mp_im_unit(void) { return complex<Real>(0.0, 1.0); }

  /*
  View an existing Eigen::Tensor of rank 2 as an Eigen::Map<Eigen::Matrix>
  Rows/Cols are determined from the matrix
  */
  template<typename Scalar>
  auto ArrayMap(Tensor<Scalar, 2>& tensor) {
      return Eigen::Map<ArrayXXr<Scalar>>(tensor.data(), tensor.dimension(0), tensor.dimension(1));
  }

  /*
  Converts an Eigen::Matrix (or expression) to Eigen::Tensor
  with dimensions specified in std::array
  */
  template<typename Derived, typename T, auto rank>
  Tensor<typename Derived::Scalar, rank>
  TensorCast(const EigenBase<Derived>& matrix, const std::array<T, rank>& dims) {
      return Eigen::TensorMap<const Tensor<const typename Derived::Scalar, rank>>
                  (matrix.derived().eval().data(), dims);
  }

  /*
  Converts an Eigen::Matrix (or expression) to Eigen::Tensor
  with dimensions as variadic arguments
  */
  template<typename Derived, typename... Dims>
  auto TensorCast(const EigenBase<Derived>& matrix, const Dims... dims) {
      static_assert(sizeof...(Dims) > 0, "TensorCast: sizeof... (Dims) must be larger than 0");
      return TensorCast(matrix, std::array<Eigen::Index, sizeof...(Dims)>{dims...});
  }

  /*
  Converts an Eigen::Matrix (or expression) to Eigen::Tensor
  with dimensions directly as arguments in a variadic template
  */
  template<typename Derived>
  auto TensorCast(const EigenBase<Derived>& matrix) {
    if constexpr(Derived::ColsAtCompileTime == 1 or Derived::RowsAtCompileTime == 1) {
      return TensorCast(matrix, matrix.size());
    } else {
      return TensorCast(matrix, matrix.rows(), matrix.cols());
    }
  }

  /* Get a readable name/description of the type given in the template argument */
  template <class Real>
  string GetTypeName() {
    string calc_type = typeid(static_cast<Real>(0)).name();
    if (calc_type.find("N5boost14multiprecision6numberINS0_8backends18mpfr_float_backend") != string::npos) {
      size_t prec_begin = calc_type.find("ILj") + 3;
      size_t prec_end = calc_type.find("ELNS");
      string precision = calc_type.substr(prec_begin, prec_end - prec_begin);
      calc_type = "custom_" + precision;
    } else if (calc_type == "f")
      calc_type = "single";
    else if (calc_type == "d")
      calc_type = "double";
    else if (calc_type == "e")
      calc_type = "quad";
    return calc_type;
  }

  /*
  Get a slice of a rank-3 tensor, so that the indices of the slices are given by Eigen::ArithmeticSequence.
  This is just a helper function for sphCheckBesselConvergence, and is not usable for general purpose.
  */
  template <class Real>
  Tensor3c<Real> TensorSlice(Tensor3c<Real>& tensor,
      ArithmeticSequence<long int, long int, long int> slice_dim1,
      ArithmeticSequence<long int, long int, long int> slice_dim2,
      ArithmeticSequence<long int, long int, long int> slice_dim3) {
    long int new_dim1 = slice_dim1.size();
    long int new_dim2 = slice_dim2.size();
    long int new_dim3 = slice_dim3.size();
    Tensor3c<Real> output(new_dim1, new_dim2, new_dim3);
    for (long int i = 0; i < new_dim1; i++) {
      for (long int j = 0; j < new_dim2; j++) {
        for (long int k = 0; k < new_dim3; k++)
          output(i, j, k) = tensor(slice_dim1[i], slice_dim2[j], slice_dim3[k]);
      }
    }
    return output;
  }

  /*
  Initialize a linearly-spaced single-column Eigen::Array by converting from an Eigen::ArithmeticSequence.
  Parameters are the same ones used to construct an Eigen::ArithmeticSequence.
  */
  ArrayXi Seq2Array(long int first, long int last, long int stride);

  /*
  Reduce a complex rank-3 Eigen::Tensor into a complex Eigen::Array by
  reducing the third dimension of the tensor down to only coefficients whose third index is `offset`,
  and reducing the first dimenstion down to only the last `num_rows` rows.
  This was used specifically as a helper function in sphCalculatePQ.
  */
  template <class Real>
  ArrayXXc<Real> Subtensor2ArrMap(const Tensor3c<Real>& tensor,
      const std::array<int, 3>& offsets, const std::array<int, 3>& extents, int rows, int cols) {
    ArrayXXc<Real> output(rows, cols);
    for (int i = 0; i < rows; i++) {
      for (int j = 0; j < cols; j++)
        output(i, j) = tensor(offsets[0] + i, offsets[1], offsets[2] + j);
    }
    return output;
  }

  /* Performs matrix inversion using LU with partial pivoting of columns. This is used in rvhGetTRfromPQ. */
  template <class Real>
  ArrayXXc<Real> InvertLUcol(MatrixXc<Real>& M) {
    PartialPivLU<MatrixXc<Real>> PLU_decomp = M.matrix().lu();
    return PLU_decomp.inverse().array();
  }

  /* Performs the equivalent of MATLAB's logical indexing on a single-column Eigen::Array. */
  template <class Real>
  ArrayXr<Real> LogicalSlice(ArrayXr<Real>& base_array, ArrayXb& bool_array) {
    assert(base_array.size() == bool_array.size());
    int output_size = bool_array.count();
    ArrayXr<Real> output(output_size);
    int base_index = 0;
    for (int i = 0; i < output_size && base_index < bool_array.size(); i++, base_index++) {
      while (!bool_array(base_index))
        base_index++;
      output(i) = base_array(base_index);
    }
    return output;
  }

  /* Performs the equivalent of MATLAB's logical indexing on a single-row Eigen::Array. */
  template <class Real>
  RowArrayXr<Real> LogicalSlice(RowArrayXr<Real>& base_array, RowArrayXb& bool_array) {
    assert(base_array.size() == bool_array.size());
    int output_size = bool_array.count();
    RowArrayXr<Real> output(output_size);
    int base_index = 0;
    for (int i = 0; i < output_size && base_index < bool_array.size(); i++, base_index++) {
      while (!bool_array(base_index))
        base_index++;
      output(i) = base_array(base_index);
    }
    return output;
  }

  /* Gets the indices of all 'true' components of a single-column boolean Eigen::Array. */
  ArrayXi LogicalIndices(ArrayXb& bool_array);

  /* Take a tensor and return the same tensor, but with all its coefficients conjugated. */
  template <class Real>
  Tensor4c<Real> TensorConj(Tensor4c<Real>& base) {
    Tensor4c<Real> output = base;
    for (int i = 0; i < base.dimension(0); i++) {
      for (int j = 0; j < base.dimension(1); j++) {
        for (int k = 0; k < base.dimension(2); k++) {
          for (int l = 0; l < base.dimension(3); l++)
            output(i, j, k, l) = conj(base(i, j, k, l));
        }
      }
    }
    return output;
  }

  /* Calculates the Bessel function of the first kind with many values of `nu` and a fixed value of `x`. */
  template <class Real>
  ArrayXr<Real> ArrBesselJ(ArrayXr<Real>& nu, Real x) {
    ArrayXr<Real> output(nu.size());
    for (int i = 0; i < nu.size(); i++)
      output(i) = boost::math::cyl_bessel_j<Real>(nu(i), x);
    return output;
  }

  /* Calculates the Bessel function of the second kind with many values of `nu` and a fixed value of `x`. */
  template <class Real>
  ArrayXr<Real> ArrBesselY(ArrayXr<Real>& nu, Real x) {
    ArrayXr<Real> output(nu.size());
    for (int i = 0; i < nu.size(); i++)
      output(i) = boost::math::cyl_neumann<Real>(nu(i), x);
    return output;
  }
}

#endif


#include <boost/random/laplace_distribution.hpp>