#ifndef TENSOR_OPS_H
#define TENSOR_OPS_H

#include <stdexcept>

#include <boost/optional.hpp>

#include <fmt/format.h>

#include <tensor.hpp>
#include <types.hpp>

constexpr index_t expand = std::numeric_limits<index_t>::max();

struct TensorError: public std::runtime_error {
    TensorError(std::string const &msg): std::runtime_error(msg) {}
};

struct MismatchedNumberOfElements: public TensorError {
    MismatchedNumberOfElements(std::size_t lhs, std::size_t rhs):
        TensorError(fmt::format("Number of elements are not the same: {} and {}", lhs, rhs)) {}
};

struct MismatchedDimensions: public TensorError {
    MismatchedDimensions(extent const &shape1, extent const &shape2):
        TensorError(fmt::format("Mismatched dimensions: {} and {}", shape1, shape2)) {}
};

struct CannotBroadcast: public TensorError {
    CannotBroadcast(extent const &shape1, extent const &shape2):
        TensorError(fmt::format("Cannot broadcast: {} and {}", shape1, shape2)) {}
};

struct NotEnoughDimensions: public TensorError {
    NotEnoughDimensions(extent const &shape):
        TensorError(fmt::format("Not enough dimensions: {}", shape)) {}
};

/**
 * @brief Returns a tensor fill with `0`s
 *
 * @tparam T The tensor type
 * @tparam Device=CPU The device tensor is stored on
 * @param shape The shape of the tensor
 * @return Tensor<T, Device>
 */
template <typename T, typename Device=CPU>
Tensor<T, Device> zeros(extent const &shape) {
    Tensor<T, Device> result(shape);
    fill(result, 0);
    return result;
}

/**
 * @brief Returns a tensor fill with `1`s
 *
 * @tparam T The tensor type
 * @tparam Device=CPU The device tensor is stored on
 * @param shape The shape of the tensor
 * @return Tensor<T, Device>
 */
template <typename T, typename Device=CPU>
Tensor<T, Device> ones(extent const &shape) {
    Tensor<T, Device> result(shape);
    fill(result, 0);
    return result;
}

template <typename T, typename Device>
void iota(Tensor<T, Device> &t, T start=0, T stride=1) {
    T value = start;
    fill(t, [&value, stride]() {
        T tmp = value;
        value += stride;
        return tmp;
    });
}

template <typename T, typename Device=CPU>
Tensor<T, Device> range(T start, T end, T stride=1) {
    std::size_t size = std::floor((end - start) / stride);
    Tensor<T, Device> result({size});

    iota(result, start, end, stride);
    return result;
}

/**
 * @brief Re-orders dimensions of Tensor
 *
 * @tparam T The tensor type
 * @tparam Device The device tensor is stored on
 * @param tensor The tensor to re-order
 * @param order The new order of dimensions
 * @return Tensor<T, Device>
 */
template <typename T, typename Device>
Tensor<T, Device> transpose(
    Tensor<T, Device> const &tensor,
    indices const &order)
{
    extent shape(tensor.shape().size());
    indices offset(tensor.shape().size());
    indices strides(tensor.shape().size());

    for (index_t i = 0; i < tensor.shape().size(); i++) {
        shape[i] = tensor.view().shape[order[i]];
        offset[i] = tensor.view().offset[order[i]];
        strides[i] = tensor.view().strides[order[i]];
    }

    return tensor.view({shape, offset, order, strides});
}

// TODO: move to source
inline boost::optional<index_t> get_inferred_dimension(extent const &shape) {
    boost::optional<index_t> result;

    for (index_t i = 0; i < shape.size(); i++) {
        if (shape[i] == expand) {
            if (!result) {
                result = i;
            } else {
                throw TensorError("Cannot infer more than one dimension.");
            }
        }
    }

    return result;
}

// TODO: move to source
inline void calculate_reshape(extent const &from_shape, extent &to_shape) {
    auto inferred_dimension = get_inferred_dimension(to_shape);

    if (inferred_dimension) {
        // number of elements for the new shape
        to_shape[*inferred_dimension] = 1;
        std::size_t new_size = num_elements(to_shape);

        // number of elements for the old shape
        std::size_t total_size = num_elements(from_shape);

        // calculate the size of the inferred dimension
        std::size_t inferred_size = total_size / new_size;
        to_shape[*inferred_dimension] = inferred_size;
    }
}

// TODO: make this either respect order, or take order f strides
template <typename T, typename Device>
Tensor<T, Device> reshape(Tensor<T, Device> const &tensor, extent const &shape) {
    extent new_shape = shape;
    calculate_reshape(tensor.shape(), new_shape);

    // we can only reshape if the number of elements is conserved
    if (num_elements(new_shape) != num_elements(tensor.shape())) {
        throw MismatchedNumberOfElements(num_elements(new_shape),
                                         num_elements(tensor.shape()));
    }

    auto order = make_row_major_order(new_shape.size());
    auto strides = make_strides(new_shape, order);
    auto offset = make_offset(new_shape.size());

    if (tensor.contiguous()) {
        return tensor.view({new_shape, offset, order, strides});
    }

    return copy(tensor.view({new_shape, offset, order, strides}));
}

template <typename T, typename Device>
bool is_broadcastable_to(Tensor<T, Device> const &tensor, extent const &shape) {
    if (shape.size() < tensor.shape().size()) return false;

    auto shape_it = shape.rbegin();
    auto tensor_shape_it = tensor.shape().rbegin();
    for (; tensor_shape_it != tensor.shape().rend(); ++shape_it, ++tensor_shape_it) {
        if (*shape_it != *tensor_shape_it && *tensor_shape_it != 1) return false;
    }

    return true;
}

namespace detail {

template <typename T, typename Device>
Tensor<T, Device> broadcast_to(Tensor<T, Device> const &tensor, extent const &shape) {
    extent strides(shape.size(), 0);
    auto offset = make_offset(shape.size());
    auto order = make_row_major_order(shape.size());

    std::size_t diff = shape.size() - tensor.shape().size();
    for (index_t i = 0; i < tensor.shape().size(); i++) {
        if (tensor.shape()[i] > 1) {
            strides[diff+i] = tensor.view().strides[i];
        } else {
            strides[diff+i] = 0;
        }
    }

    return tensor.view({shape, offset, order, strides});
}

} // namespace detail

template <typename T, typename Device>
Tensor<T, Device> broadcast_to(Tensor<T, Device> const &tensor, extent const &shape) {
    if (tensor.shape() == shape) return tensor;

    if (!is_broadcastable_to(tensor, shape)) {
        throw CannotBroadcast(tensor.shape(), shape);
    }

    return detail::broadcast_to(tensor, shape);
}

template <typename T, typename Device>
std::pair<Tensor<T, Device>, Tensor<T, Device>>
broadcast(Tensor<T, Device> const &t1, Tensor<T, Device> const &t2) {
    if (t1.shape() == t2.shape()) return std::make_pair(t1, t2);

    bool t1_to_t2 = is_broadcastable_to(t1, t2.shape());
    bool t2_to_t1 = is_broadcastable_to(t2, t1.shape());

    if (!t1_to_t2 && !t2_to_t1) {
        throw CannotBroadcast(t1.shape(), t2.shape());
    }

    auto result1 = t1_to_t2 ? detail::broadcast_to(t1, t2.shape()) : t1;
    auto result2 = t2_to_t1 ? detail::broadcast_to(t2, t1.shape()) : t2;
    return std::make_pair(result1, result2);
}

template <typename RT, typename T, typename Device, typename F>
Tensor<RT, Device> apply(Tensor<T, Device> const &lhs,
                         Tensor<T, Device> const &rhs,
                         F fn)
{
    if (lhs.shape() != rhs.shape()) {
        // if the dimensions don't match, try to broadcast
        auto [lhs_broadcast, rhs_broadcast] = broadcast(lhs, rhs);
        return apply<RT>(lhs_broadcast, rhs_broadcast, fn);
    }

    Tensor<RT, Device> result(lhs.shape());

    // can optimize if both lhs and rhs are contiguous
    for (auto const &index : lhs.indices()) {
        result(index) = fn(lhs(index), rhs(index));
    }

    return result;
}

template <typename T, typename Device, typename F>
void iapply(Tensor<T, Device> &lhs, Tensor<T, Device> const &rhs, F fn) {
    if (lhs.shape() != rhs.shape()) {
        throw MismatchedDimensions(lhs.shape(), rhs.shape());
    }

    // can optimize if both lhs and rhs are contiguous
    for (auto const &index : lhs.indices()) {
        lhs(index) = fn(lhs(index), rhs(index));
    }
}

template <typename RT, typename T, typename Device, typename F>
Tensor<RT, Device> apply(Tensor<T, Device> const &t, F fn) {
    Tensor<RT, Device> result(t.shape());
    for (auto const &index : t.indices()) {
        result(index) = fn(t(index));
    }

    return result;
}

template <typename T, typename Device, typename F>
void iapply(Tensor<T, Device> &t, F fn) {
    for (auto const &index : t.indices()) {
        t(index) = fn(t(index));
    }
}

/**
 * @brief Sets all the elements of a tensor to the given value
 *
 * @tparam T The tensor type
 * @tparam Device The device tensor is stored on
 * @param t The tensor to fill
 * @param value The value use
 */
template <typename T, typename Device>
void fill(Tensor<T, Device> &t, T const &value) {
    iapply(t, [value](T const &) { return value; });
}

/**
 * @brief Fills tensor using a successive calls to passed in function
 *
 * @tparam T The tensor type
 * @tparam Device The device tensor is stored on
 * @tparam F Function type
 * @param t The tensor to fill
 * @param fn The function used to fill tensor
 */
template <typename T, typename Device, typename F>
void fill(Tensor<T, Device> &t, F fn) {
    iapply(t, [fn](T const &) { return fn(); });
}

template <typename T, typename Device>
Tensor<T, Device> operator +(Tensor<T, Device> const &lhs, Tensor<T, Device> const &rhs) {
    return apply<T>(lhs, rhs, [](T const &lop, T const &rop) { return lop + rop; });
}

template <typename T, typename Device>
Tensor<T, Device> &operator +=(Tensor<T, Device> &lhs, Tensor<T, Device> const &rhs) {
    iapply(lhs, rhs, [](T const &lop, T const &rop) { return lop + rop; });
    return lhs;
}

template <typename T, typename Device>
Tensor<T, Device> operator -(Tensor<T, Device> const &lhs, Tensor<T, Device> const &rhs) {
    return apply<T>(lhs, rhs, [](T const &lop, T const &rop) { return lop - rop; });
}

template <typename T, typename Device>
Tensor<T, Device> &operator -=(Tensor<T, Device> &lhs, Tensor<T, Device> const &rhs) {
    iapply(lhs, rhs, [](T const &lop, T const &rop) { return lop - rop; });
    return lhs;
}

template <typename T, typename Device>
Tensor<T, Device> operator *(ElementTensor<T, Device> lhs, ElementTensor<T, Device> rhs) {
    return apply<T>(lhs.tensor, rhs.tensor, [](T const &lop, T const &rop) {
        return lop * rop;
    });
}

template <typename T, typename Device>
Tensor<T, Device> &operator *=(ElementTensor<T, Device> lhs, ElementTensor<T, Device> rhs) {
    iapply(lhs.tensor, rhs.tensor, [](T const &lop, T const &rop) { return lop * rop; });
    return lhs.tensor;
}

template <typename T, typename Device>
Tensor<T, Device> operator /(Tensor<T, Device> const &lhs, Tensor<T, Device> const &rhs) {
    return apply<T>(lhs, rhs, [](T const &lop, T const &rop) { return lop / rop; });
}

template <typename T, typename Device>
Tensor<T, Device> &operator /=(Tensor<T, Device> &lhs, Tensor<T, Device> const &rhs) {
    iapply(lhs, rhs, [](T const &lop, T const &rop) { return lop / rop; });
    return lhs;
}

template <typename T, typename Device>
Tensor<T, Device> sin(Tensor<T, Device> &t) {
    return apply<T>(t, [](T const &v) { return std::sin(v); });
}

template <typename T, typename Device>
void isin(Tensor<T, Device> &t) {
    iapply(t, [](T const &v) { return std::sin(v); });
}

// move to detail
template <typename T, typename Device>
Tensor<T, Device> vector_vector_product(Tensor<T, Device> const &lhs,
                                        Tensor<T, Device> const &rhs)
{
    if (num_elements(lhs) != num_elements(rhs)) {
        throw MismatchedNumberOfElements(num_elements(lhs), num_elements(rhs));
    }

    T result(0);
    for (index_t i = 0; i < num_elements(lhs); i++) {
        result += lhs(i)*rhs(i);
    }

    return tensor({result});
}

template <typename T, typename Device>
Tensor<T, Device> matrix_vector_product(Tensor<T, Device> const &lhs,
                                        Tensor<T, Device> const &rhs)
{
    // verify inner dimensions match: (Nx1, 1)
    Tensor<T, Device> result({lhs.shape()[0]});
    fill(result, 0);

    for (index_t i = 0; i < lhs.shape()[0]; i++) {
        for (index_t j = 0; j < lhs.shape()[1]; j++) {
            result(i) += lhs(i, j)*rhs(j);
        }
    }

    return result;
}

// need batch_matrix_vector

template <typename T, typename Device>
Tensor<T, Device> matrix_matrix_product(Tensor<T, Device> const &lhs,
                                        Tensor<T, Device> const &rhs)
{
    Tensor<T, Device> result({lhs.shape()[0], rhs.shape()[1]});
    fill(result, 0);

    for (index_t i = 0; i < lhs.shape()[0]; i++) {
        for (index_t j = 0; j < lhs.shape()[1]; j++) {
            for (index_t k = 0; k < rhs.shape()[1]; k++) {
                result(i, k) += lhs(i, j)*rhs(j, k);
            }
        }
    }

    return result;
}

template <typename T, typename Device>
Tensor<T, Device> batch_matrix_matrix_product(Tensor<T, Device> const &lhs,
                                              Tensor<T, Device> const &rhs)
{
    // BxMxN * BxNxP
    Tensor<T, Device> result({lhs.shape()[0], lhs.shape()[1], rhs.shape()[2]});
    fill(result, 0);

    for (index_t b = 0; b < lhs.shape()[0]; b++) {
        for (index_t i = 0; i < lhs.shape()[1]; i++) {
            for (index_t j = 0; j < lhs.shape()[2]; j++) {
                for (index_t k = 0; k < rhs.shape()[2]; k++) {
                    result(b, i, k) += lhs(b, i, j)*rhs(b, j, k);
                }
            }
        }
    }

    return result;
}

inline extent get_batch_shape(extent const &shape) {
    if (shape.size() < 3) throw NotEnoughDimensions(shape);
    std::size_t count = shape.size() - 2;

    extent batch_shape(count);
    std::copy_n(std::begin(shape), count, std::begin(batch_shape));
    return batch_shape;
}

inline extent calculate_batch_shape(extent const &shape, index_t d0, index_t d1) {
    auto new_shape = get_batch_shape(shape);
    new_shape.push_back(d0);
    new_shape.push_back(d1);
    return new_shape;
}

template <typename T, typename Device>
Tensor<T, Device> product(Tensor<T, Device> const &lhs,
                          Tensor<T, Device> const &rhs)
{
    auto lhs_dims = num_dims(lhs);
    auto rhs_dims = num_dims(rhs);

    // 5x3x6 * 5x
    if (lhs_dims > 2 || rhs_dims > 2) {
        auto lhs_copy = lhs;
        auto rhs_copy = rhs;

        // flatten batch dimension if either tensor is > 3 dims
        if (lhs_dims > 3) {
            lhs_copy = reshape(lhs_copy, {expand, lhs.shape()[lhs_dims-2], lhs.shape()[lhs_dims-1]});
        }

        if (rhs_dims > 3) {
            rhs_copy = reshape(rhs_copy, {expand, rhs.shape()[rhs_dims-2], rhs.shape()[rhs_dims-1]});
        }

        // if (lhs_dims == 2) {
        //     // add batch dimension
        // } else if (lhs_dims == 1) {
        //     // treat as batch matrix-vector multiplication
        // }

        // 2. either lhs or rhs > 3, in which case need to flatten
        auto result = batch_matrix_matrix_product(lhs_copy, rhs_copy);

        // for now, assume both lhs and rhs start off as the same shape
        // auto new_shape = get_batch_size(orig_lhs_shape);
        auto new_shape = calculate_batch_shape(lhs.shape(), result.shape()[1], result.shape()[2]);

        // reshape into original batch dims
        return reshape(result, new_shape);
    }

    if (lhs_dims == 1 && rhs_dims == 1) {
        return vector_vector_product(lhs, rhs);
    }

    if (rhs_dims == 1) {
        return matrix_vector_product(lhs, rhs);
    }

    if (lhs_dims == 1) {
        // TODO: follow pytorch convention and add a dimension
        // to lhs and make matrix_matrix?
        return matrix_vector_product(rhs, lhs);
    }

    // matrix output
    return matrix_matrix_product(lhs, rhs);
}

template <typename T, typename Device>
Tensor<T, Device> operator *(Tensor<T, Device> const &lhs, Tensor<T, Device> const &rhs) {
    return product(lhs, rhs);
}

template <typename T, typename Device>
Tensor<std::uint8_t, Device> operator ==(Tensor<T, Device> const &lhs,
                                         Tensor<T, Device> const &rhs)
{
    return apply<std::uint8_t>(lhs, rhs, [](T const &lop, T const &rop) {
        return lop == rop;
    });
}

template <typename T, typename Device>
Tensor<std::uint8_t, Device> operator !=(Tensor<T, Device> const &lhs,
                                         Tensor<T, Device> const &rhs)
{
    return !(lhs == rhs);
}

template <typename T, typename Device, typename F>
bool all(Tensor<T, Device> const &op, F fn) {
    for (auto const &index : op.indices()) {
        if (!fn(op(index))) return false;
    }

    return true;
}

template <typename T, typename Device, typename F>
bool any(Tensor<T, Device> const &op, F fn) {
    for (auto const &index : op.indices()) {
        if (fn(op(index))) return true;
    }

    return false;
}

template <typename T, typename Device>
bool equals(Tensor<T, Device> const &lhs, Tensor<T, Device> const &rhs) {
    return all(lhs == rhs, [](std::uint8_t v) { return v == true; });
}

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/log1p.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/oneminus.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( atanh_
                          , (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 absa0 = bs::abs(a0);
        A0 t =  absa0+absa0;
        A0 z1 = oneminus(absa0);
        auto test =  is_less(absa0, Half<A0>());
        A0 tmp = if_else(test, absa0, t)/z1;
        return bitwise_xor(bitofsign(a0), Half<A0>()*log1p(if_else(test, fma(t,tmp,t), tmp)));
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF( atanh_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const raw_tag &,
                                      const A0& a0) const BOOST_NOEXCEPT
      {
        return  Half<A0>()*log(inc(a0)/oneminus(a0));
      }
   };

} } }

#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines the class Rotation.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/Tensor.hpp"
#include "Utilities/TypeTraits/RemoveReferenceWrapper.hpp"

/// \cond
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace domain {
namespace CoordinateMaps {

/// \cond HIDDEN_SYMBOLS
template <size_t Dim>
class Rotation;
/// \endcond

/*!
 * \ingroup CoordinateMapsGroup
 * \brief Spatial rotation in two dimensions.
 *
 * Let \f$(R,\Phi)\f$ be the polar coordinates associated with
 * \f$(\xi,\eta)\f$.
 * Let \f$(r,\phi)\f$ be the polar coordinates associated with \f$(x,y)\f$.
 * Applies the spatial rotation \f$\phi = \Phi + \alpha\f$.
 *
 * The formula for the mapping is:
 *\f{eqnarray*}
  x &=& \xi \cos \alpha - \eta \sin \alpha \\
  y &=& \xi \sin \alpha + \eta \cos \alpha
  \f}.
 */
template <>
class Rotation<2> {
 public:
  static constexpr size_t dim = 2;

  /// Constructor.
  ///
  /// \param rotation_angle the angle \f$\alpha\f$ (in radians).
  explicit Rotation(double rotation_angle);
  Rotation() = default;
  ~Rotation() = default;
  Rotation(const Rotation&) = default;
  Rotation& operator=(const Rotation&) = default;
  Rotation(Rotation&&) noexcept = default;  // NOLINT
  Rotation& operator=(Rotation&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 2> operator()(
      const std::array<T, 2>& source_coords) const noexcept;

  boost::optional<std::array<double, 2>> inverse(
      const std::array<double, 2>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 2, Frame::NoFrame> jacobian(
      const std::array<T, 2>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 2, Frame::NoFrame> inv_jacobian(
      const std::array<T, 2>& source_coords) const noexcept;

  void pup(PUP::er& p);  // NOLINT

  bool is_identity() const noexcept { return is_identity_; }

 private:
  friend bool operator==(const Rotation<2>& lhs,
                         const Rotation<2>& rhs) noexcept;

  double rotation_angle_{std::numeric_limits<double>::signaling_NaN()};
  tnsr::ij<double, 2, Frame::Grid> rotation_matrix_{
      std::numeric_limits<double>::signaling_NaN()};
  bool is_identity_{false};
};

bool operator!=(const Rotation<2>& lhs, const Rotation<2>& rhs) noexcept;

/*!
 * \ingroup CoordinateMapsGroup
 * \brief Spatial rotation in three dimensions using Euler angles
 *
 * Rotation angles should be specified in degrees.
 * First rotation \f$\alpha\f$ is about z axis.
 * Second rotation \f$\beta\f$ is about rotated y axis.
 * Third rotation \f$\gamma\f$ is about rotated z axis.
 * These rotations are of the \f$(\xi,\eta,\zeta)\f$ coordinate system with
 * respect
 * to the grid coordinates \f$(x,y,z)\f$.
 *
 * The formula for the mapping is:
 * \f{eqnarray*}
 *   x &=& \xi (\cos\gamma \cos\beta \cos\alpha - \sin\gamma \sin\alpha)
 * + \eta (-\sin\gamma \cos\beta \cos\alpha - \cos\gamma \sin\alpha)
 * + \zeta \sin\beta \cos\alpha \\
 * y &=& \xi (\cos\gamma \cos\beta \sin\alpha + \sin\gamma \cos\alpha)
 * + \eta (-\sin\gamma \cos\beta \sin\alpha + \cos\gamma \cos\alpha)
 * + \zeta \sin\beta \sin\alpha \\
 *        z &=& -\xi \cos\gamma \sin\beta + \eta \sin\gamma \sin\beta
 *        +  \zeta \cos\beta
 *  \f}
 */
template <>
class Rotation<3> {
 public:
  static constexpr size_t dim = 3;

  /// Constructor.
  ///
  /// \param rotation_about_z the angle \f$\alpha\f$ (in radians).
  /// \param rotation_about_rotated_y the angle \f$\beta\f$ (in radians).
  /// \param rotation_about_rotated_z the angle \f$\gamma\f$ (in radians).
  Rotation(double rotation_about_z, double rotation_about_rotated_y,
           double rotation_about_rotated_z);
  Rotation() = default;
  ~Rotation() = default;
  Rotation(const Rotation&) = default;
  Rotation& operator=(const Rotation&) = default;
  Rotation(Rotation&&) noexcept = default;  // NOLINT
  Rotation& operator=(Rotation&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  void pup(PUP::er& p);  // NOLINT

  bool is_identity() const noexcept { return is_identity_; }

 private:
  friend bool operator==(const Rotation<3>& lhs,
                         const Rotation<3>& rhs) noexcept;

  double rotation_about_z_{std::numeric_limits<double>::signaling_NaN()};
  double rotation_about_rotated_y_{
      std::numeric_limits<double>::signaling_NaN()};
  double rotation_about_rotated_z_{
      std::numeric_limits<double>::signaling_NaN()};
  tnsr::ij<double, 3, Frame::Grid> rotation_matrix_{
      std::numeric_limits<double>::signaling_NaN()};
  bool is_identity_{false};
};

bool operator!=(const Rotation<3>& lhs, const Rotation<3>& rhs) noexcept;

}  // namespace CoordinateMaps
}  // namespace domain


#include <stdio.h>
#include <math.h>
#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <SymEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>
#include <time.h>
#include <Python.h>
#include <numpy/arrayobject.h>
using namespace Eigen;
using namespace Spectra;

void make_top_eigenvectors(double *val,double *ind, int KK, int NN, int NK, double *eigenvectors,double *eigenvalues){
    clock_t begin = clock();
    Eigen::SparseMatrix<double> mat((const int) NN,(const int) NN);         // default is column major
    mat.reserve(Eigen::VectorXi::Constant((const int) NN, (const int) NK));
    typedef Eigen::Triplet<double> T;
    std::vector<T> tripletList;
    tripletList.reserve((const int) NN*NK);
    for(int i=0; i<NN; i++){
        for (int j = 0; j< NK; j++){
            tripletList.push_back(T((int) ind[i*NK+j],i,val[i*NK+j]));
            //std::cout << ind[i*NK+j] << std::endl << val[i*NK+j] << std::endl;
        }
    }
    mat.setFromTriplets(tripletList.begin(), tripletList.end());
    mat += Eigen::SparseMatrix<double>(mat.transpose());
    clock_t end = clock();
    //printf("Elapsed time in initialization is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    SparseSymMatProd<double> op(mat);
    begin = clock();
    // Construct eigen solver object, requesting the largest KK eigenvalues
    SymEigsSolver< double, LARGEST_ALGE, SparseSymMatProd<double> > eigs(&op, KK, 2*KK);
    // Initialize and compute
    eigs.init();
    int nconv = eigs.compute();
    // Retrieve results

    Eigen::VectorXd evalues;
    Eigen::MatrixXd evectors;
    if(eigs.info() == SUCCESSFUL){
        evalues = eigs.eigenvalues();
        evectors = eigs.eigenvectors();
    }
    //std::cout << "Eigenvalues found:\n" << evalues << std::endl;
    end = clock();
    //printf("Elapsed time in eigen-decomposition is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
    begin = clock();
    for (int j = 0; j< KK; j++){
        eigenvalues[j] = evalues[j];
        for (int i = 0; i < NN; i++){
            eigenvectors[i*KK+j] = evectors.col(j)[i];
        }
    }
   end = clock();
    //printf("Elapsed time in copying eigenvectors is %f seconds\n", (double)(end - begin)/CLOCKS_PER_SEC);
    ///
}

PyObject * top_eig(PyObject *in0, PyObject *in1, PyObject *scal){
//PyObject * top_eig(PyArrayObject *in0, PyArrayObject *in1, PyObject *scal){
    //PyArrayObject *in0_arr = NULL;
    PyArrayObject *in0_arr = (PyArrayObject *)in0;
    //in0_arr = PyArray_GETCONTIGUOUS((PyArrayObject *)in0);
    //in0_arr = PyArray_GETCONTIGUOUS(in0);

    double *val = NULL;
    val = (double *)PyArray_DATA(in0_arr);

    //PyArrayObject *in1_arr = NULL;
    PyArrayObject *in1_arr = (PyArrayObject *)in1;
    //in1_arr = PyArray_GETCONTIGUOUS((PyArrayObject *)in1);
    //in1_arr = PyArray_GETCONTIGUOUS(in1);

    double *ind = NULL;
    ind = (double *)PyArray_DATA(in1_arr);

    int KK = (int)PyLong_AsLong(scal);

    int NN, NK;
    npy_intp *dims = NULL;
    dims = PyArray_DIMS(in0_arr);
    NN = dims[0];

    dims = PyArray_DIMS(in1_arr);
    NK = dims[1];

    dims[0] = (npy_intp)NN;
    dims[1] = (npy_intp)KK;

    PyObject *out1 = NULL;
    out1 = PyArray_SimpleNew(2, dims, NPY_DOUBLE);

    PyArrayObject *out1_arr = NULL;
    out1_arr = PyArray_GETCONTIGUOUS((PyArrayObject *)out1);

    double *eigenvectors = NULL;
    eigenvectors = (double *)PyArray_DATA(out1_arr);

    dims[0] = dims[1];
    int tmp = 1;
    dims[1] = (npy_intp)tmp;

    PyObject *out2 = NULL;
    out2 = PyArray_SimpleNew(2, dims, NPY_DOUBLE);

    PyArrayObject *out2_arr = NULL;
    out2_arr = PyArray_GETCONTIGUOUS((PyArrayObject *)out2);

    double *eigenvalues = NULL;
    eigenvalues = (double *)PyArray_DATA(out2_arr);

    make_top_eigenvectors(val, ind, KK, NN, NK, eigenvectors, eigenvalues);

    PyObject *outs;
    outs = PyTuple_New(2);
    PyTuple_SetItem(outs, 0, (PyObject *)out1_arr);
    PyTuple_SetItem(outs, 1, (PyObject *)out2_arr);
    return outs;
}

static PyObject * _top_eig(PyObject *self, PyObject *args){
    PyObject *in0, *in1;
    //PyArrayObject *in0, *in1;
    PyObject *scal = NULL;
    /*if (!PyArg_ParseTuple(args, "O!O!O", &PyArray_Type, &in0, &PyArray_Type, &in1, &scal)){
        return NULL;
    }*/
    if (!PyArg_ParseTuple(args, "OOO", &in0, &in1, &scal)){
        return NULL;
    }

    in0 = PyArray_FROM_OTF(in0, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    in1 = PyArray_FROM_OTF(in1, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);

    PyObject *out = NULL;
    out = top_eig(in0, in1, scal);
    return out;
}

static PyMethodDef topeigMethods[] = {
    {
        "top_eig",
        _top_eig,
        METH_VARARGS,
        ""
    },
    {NULL, NULL, 0, NULL}
};

static struct PyModuleDef topeigModule = {
    PyModuleDef_HEAD_INIT,
    "topeigModule",
    NULL,
    -1,
    topeigMethods
};

PyMODINIT_FUNC PyInit_top_eig(void){
    import_array();
    return PyModule_Create(&topeigModule);
}


#include "global_constants.hh"
#include "aux.hh"
#include "stochastic.hh"
#include "matrix/utility.hh"

#include "cad_utility.hh"

#include <armadillo>

#include "Ins.hh"
#include "GPS.hh"
GPS_FSW::GPS_FSW()
:   time(time_management::get_instance()),
    MATRIX_INIT(FF, 8, 8),
    MATRIX_INIT(PHI, 8, 8),
    MATRIX_INIT(PP, 8, 8),
    MATRIX_INIT(PP0, 8, 8),
    VECTOR_INIT(SXH, 3),
    VECTOR_INIT(VXH, 3),
    VECTOR_INIT(CXH, 3),
    VECTOR_INIT(ZZ, 8),
    VECTOR_INIT(WEII, 3)
{}

GPS_FSW::GPS_FSW(const GPS_FSW &other)
:   time(time_management::get_instance()),
    MATRIX_INIT(FF, 8, 8),
    MATRIX_INIT(PHI, 8, 8),
    MATRIX_INIT(PP, 8, 8),
    MATRIX_INIT(PP0, 8, 8),
    VECTOR_INIT(SXH, 3),
    VECTOR_INIT(VXH, 3),
    VECTOR_INIT(CXH, 3),
    VECTOR_INIT(ZZ, 8),
    VECTOR_INIT(WEII, 3)
{}

GPS_FSW & GPS_FSW::operator= (const GPS_FSW &other) {
    if (&other == this)
        return *this;

    return *this;
}

void GPS_FSW::setup_state_covariance_matrix(double factp, double pclockb, double pclockf) {
    PP = arma::mat88(arma::fill::zeros);
    for (int i = 0; i < 3; i++) {
        PP(i, i)         = pow(ppos * (1 + factp), 2);
        PP(i + 3, i + 3) = pow(pvel * (1 + factp), 2);
    }
    PP(6, 6) =  pow(pclockb * (1 + factp), 2);
    PP(7, 7) =  pow(pclockf * (1 + factp), 2);

    return;
}

void GPS_FSW::setup_error_covariance_matrix(double factq, double qclockb, double qclockf) {
    this->factq = factq;
    this->qclockb = qclockb;
    this->qclockf = qclockf;

    return;
}

void GPS_FSW::setup_fundamental_dynamic_matrix(double uctime_cor) {
    // fundamental dynamic matrix of filter - constant throughout
    FF = arma::mat88(arma::fill::zeros);
    FF(0, 3) = 1;
    FF(1, 4) = 1;
    FF(2, 5) = 1;
    FF(6, 7) = 1;
    FF(7, 7) = -1 / uctime_cor;

    return;
}

void GPS_FSW::initialize(double int_step) {
    // state transition matrix - constant throughout
    PHI = arma::mat88(arma::fill::eye) + FF * int_step + FF * FF * (int_step * int_step / 2);
    this->WEII.zeros();
    // Due to RNP so that earth rate have 3 axis components
    this->WEII(0) = WEII1;
    this->WEII(1) = WEII2;
    this->WEII(2) = WEII3;
    // initializing update clock
    // gps_epoch = get_elapsed_time();
}

void GPS_FSW::filter_extrapolation(double int_step) {
    arma::mat88 QQ(arma::fill::zeros);  // local

    //*** user-clock frequency and bias error growth between updates ***
    // integrating 'ucfreq_noise' Markov process to
    //  obtain user-clock bias error 'ucbias_error' (trapezoidal
    //  integration)
    // user-clock bias is updated at filter update epoch
    ucfreq_error = ucfreq_noise;
    ucbias_error = ucbias_error + (ucfreq_error + ucfreqm) * (int_step / 2);
    ucfreqm = ucfreq_error;

    //*** filter extrapolation ***
    // dynamic error covariance matrix
    for (int i = 0; i < 3; i++) {
        QQ(i, i)         = pow(qpos * (1 + factq), 2);
        QQ(i + 3, i + 3) = pow(qvel * (1 + factq), 2);
    }
    QQ(6, 6) = pow(qclockb * (1 + factq), 2);
    QQ(7, 7) = pow(qclockf * (1 + factq), 2);

    // covariance estimate extrapolation
    PP = PHI * (PP + QQ * (int_step / 2)) * trans(PHI) + QQ * (int_step / 2);

    // diagnostics: st. deviations of the diagonals of the covariance matrix
    std_pos = sqrt(PP(0, 0));
    std_vel = sqrt(PP(3, 3));
    std_ucbias = sqrt(PP(6, 6));
}

void GPS_FSW::measure(double int_step) {
    // double dtime_gps;
    // /* Testing GPS timing for update and acquire */
    // if(!gps_acq)
    //     // saving delay-time for GPS signal acquisition
    //     dtime_gps = gps_acqtime;
    // else
    //     // saving delay-time for GPS update
    //     dtime_gps = gps_step;
    // // checking when GPS update time has occured in order to initiate update
    // time_gps = get_elapsed_time() - gps_epoch;
    // if (time_gps < dtime_gps) {
    //     return;
    // }
    gps_step = int_step;
    // gps_acq = true;
    // gps_update = 1;
    // // resetting update clock
    // time_gps = 0;
    // gps_epoch = get_elapsed_time();

    // /* GPS Update and Measurement */
    // double slotm(0);

    // arma::vec8 ZZ(arma::fill::zeros);
    arma::vec8 XH(arma::fill::zeros);           // local
    arma::mat88 RR(arma::fill::zeros);          // local
    arma::mat88 HH(arma::fill::zeros);          // local
    arma::vec3 pos(arma::fill::zeros);
    arma::vec3 vel(arma::fill::zeros);
    arma::vec2 clk(arma::fill::zeros);

    // arma::vec4 pesudo_range(arma::fill::zeros);
    // arma::vec4 pesudo_range_rate(arma::fill::zeros);
    arma::vec4 channel_id(arma::fill::zeros);
    arma::mat33 TEIC = grab_TEIC();

    transmit_channel* trans_chan = grab_transmit_data();
    // arma::mat33 TEI = environment->get_TEI();
    int ii(0);
  // Pseudo-range and range-rate measurements
    for (int i = 0; i < MAX_CHAN; i++) {
        if (trans_chan[i].prn > 0) {
            // pesudo_range(ii) = gps_con->chan[i].rho0.range;
            // pesudo_range_rate(ii) = gps_con->chan[i].rho0.rate;
            channel_id(ii) = i;
            if (ii == 3) break;
            ii++;
        }
    }

    // arma::vec3 SBII = newton->get_SBII();
    // arma::vec3 VBII = newton->get_VBII();
    // arma::vec3 WEII = euler->get_WEII();

    arma::vec3 SBIIC = grab_SBIIC();
    arma::vec3 VBIIC = grab_VBIIC();
    arma::vec3 WBICI = grab_WBICI();
    arma::vec3 SBEEC = grab_SBEEC();
    arma::vec3 VBEEC = grab_VBEEC();

    // Pseudo-range and range-rate measurements
    for (int i = 0; i < 4; i++) {
        // unpacking i-th SV information
        int id(channel_id(i));

        // double dsb = gps_con->chan[id].rho0.range;
        // // measured pseudo-range
        // double dsb_meas = dsb ;//+ PR_BIAS[i] + PR_NOISE[i];// + ucbias_error;

        // double dvsb = gps_con->chan[id].rho0.rate;
        // // measured delta-range rate
        // double dvsb_meas = dvsb ;//+ DR_NOISE[i] ;//+ ucfreq_error;

        // // INS derived range measurements
        // arma::vec3 SSBIC;
        // // SSBIC = (trans(TEIC) * gps_con->chan[id].rho0.pos - SBIIC)- SPEED_OF_LIGHT * gps_con->chan[id].rho0.clk(0);
        // SSBIC = (gps_con->chan[id].rho0.pos - SBEEC);
        // double dsbc = norm(SSBIC) - SPEED_OF_LIGHT * gps_con->chan[id].rho0.clk(0);

        // arma::vec3 USSBI;
        // USSBI = SSBIC/norm(SSBIC);

        // // double dvsbc = dot(trans(TEIC) * (gps_con->chan[i].rho0.vel + cross(WEII, gps_con->chan[id].rho0.pos)), (trans(TEIC) * gps_con->chan[id].rho0.pos - SBIIC))/dsbc;
        // double dvsbc = dot(gps_con->chan[i].rho0.vel , SSBIC)/dsbc;

        // Pesudo-range on ECI/////////////////////////////////////////////////////////////////////////
        // arma::vec3 SSBI;
        // SSBI = (trans(TEIC) * gps_con->chan[id].rho0.pos - SBIIC);

        for (int j = 0; j < 3; j++) {
            pos(j) = trans_chan[id].pos[j];
            vel(j) = trans_chan[id].vel[j];
        }
        for (int k = 0; k < 2; k++) {
            clk(k) = trans_chan[id].clk[k];
        }

        arma::vec3 SSBIC;
        SSBIC = (trans(TEIC) * pos - SBIIC);

        double dsb_meas = trans_chan[id].range;  // norm(SSBI) - SPEED_OF_LIGHT * gps_con->chan[id].rho0.clk(0);

        arma::vec3 velECI;
        velECI = trans(TEIC) * (vel + cross(WEII, pos));

        double dvsb_meas = dot(velECI, SSBIC)/dsb_meas;
        /////////////////////////////////////////////////////////////////////////////////////////////////

        double dsbc = norm(SSBIC) - SPEED_OF_LIGHT * clk(0);

        double dvsbc = dot(velECI, SSBIC)/dsbc;

        arma::vec3 USSBI;
        USSBI = SSBIC/norm(SSBIC);

        ZZ[i] = dsb_meas - dsbc;
        ZZ[i + 4] = dvsb_meas - dvsbc;

        // observation matrix of filter
        for (int j = 0; j < 3; j++) {
            HH(i, j) = USSBI(j);
            HH(i + 4, j + 3) = USSBI(j) * gps_step;
        }
        HH(i, 6) = 1;
        HH(i + 4, 7) = gps_step;

        // for diagnostics: loading the 4 SV slot # of the quadriga
        // *(slot + i) = *(ssii_quad + 4 * i + 3);
        // // accumulating sum of slots
        // slotm = slotm + slot[i];
    }
    // for diagnostics displaying the SV slot# of the quadriga on the
    // console
    // but only if they have changed (i.e., sum of slot# has changed)

    // if (slotsum != slotm) {
    //     slotsum = slotm;
    //     std::cout << " *** GPS Quadriga slot # " << slot[0] << "  "
    //               << slot[1] << "  " << slot[2] << "  " << slot[3]
    //               << " ;  GDOP = " << gdop << " m ***\n";
    // }
    //*** filter correction and update (to INS: 'SXH' and 'VXH') ***
    // filter gain
    arma::mat88 KK(arma::fill::zeros);
    arma::mat88 E(arma::fill::eye);
    // measurement noise covariance matrix
    for (int i = 0; i < 4; i++) {
        RR(i, i) = pow(rpos * (1 + factr), 2);
        RR(i + 4, i + 4) = pow(rvel * (1 + factr), 2);
    }
    // Kalman gain
    KK = PP * trans(HH) * inv(HH * PP * trans(HH) + RR);
    // state correction
    XH = KK * ZZ;
    // covariance correction for next cycle
    PP = (E - KK * HH) * PP;

    // clock error bias update
    ucbias_error = ucbias_error - XH(6, 0);

    // diagnostics of 1st SV of quadriga saved to plot file
    gps_pos_meas = ZZ(0, 0);
    gps_vel_meas = ZZ(4, 0);

    // decomposing state vector for output
    for (int m = 0; m < 3; m++) {
        SXH(m, 0) = XH(m, 0);
        VXH(m, 0) = XH(m + 3, 0);
    }
    CXH(0, 0) = XH(6, 0);
    CXH(1, 0) = XH(7, 0);

    // diagnostic
    state_pos = norm(SXH);
    state_vel = norm(VXH);
}

arma::vec3 GPS_FSW::get_SXH() { return SXH; }
arma::vec3 GPS_FSW::get_VXH() { return VXH; }
arma::vec3 GPS_FSW::get_CXH() { return CXH; }


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_AUTODIF_NV_HPP
#define ODE_AUTODIF_NV_HPP

// ODE (input and output : autodif type) (not verified)

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/psa.hpp>
#include <kv/autodif.hpp>
#include <kv/ode-param.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
ode_nv(F f, ub::vector< autodif<T> >& init, const T& start, T& end, ode_param<T> p = ode_param<T>()) {
	int n = init.size();
	int i, j, k, km;

	ub::vector< psa< autodif<T> > > x, y;
	psa< autodif<T> > torg;
	psa< autodif<T> > t;

	T deltat;
	ub::vector< autodif<T> > result, new_init;

	T m;

	T radius, radius_tmp;
	T tolerance;
	int n_rad;

	ub::matrix<T> save;

	bool save_mode, save_uh, save_rh;

	new_init = autodif<T>::compress(init, save);

	m = 1.;
	for (i=0; i<n; i++) {
		using std::abs;
		m = std::max(m, abs(new_init(i).v));
		new_init(i).d.resize(n);
		for (j=0; j<n; j++) {
			using std::abs;
			m = std::max(m, abs(new_init(i).d(j)));
		}
	}
	tolerance = m * p.epsilon;

	x = new_init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< autodif<T> >::mode();
	save_uh = psa< autodif<T> >::use_history();
	save_rh = psa< autodif<T> >::record_history();
	psa< autodif<T> >::mode() = 1;
	psa< autodif<T> >::use_history() = false;
	psa< autodif<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< autodif<T> >::record_history() = true;
	psa< autodif<T> >::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< autodif<T> >::use_history() = true;
		if (j == p.order - 1) psa< autodif<T> >::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			y(i) = setorder(y(i), j+1);
		}
		x = new_init + y;
	}

	if (p.autostep) {
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				using std::abs;
				m = std::max(m, abs(x(i).v(j).v));
				#ifdef IGNORE_DIF_PART
				#else
				km = x(i).v(j).d.size();
				for (k=0; k<km; k++) {
					using std::abs;
					m = std::max(m, abs(x(i).v(j).d(k)));
				}
				#endif
			}
			if (m == 0.) continue;
			radius_tmp = std::pow((double)m, 1./j);
			if (radius_tmp > radius) radius = radius_tmp;
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
	}

	deltat = end - start;

	if (p.autostep && radius < deltat) {
		end = start + radius;
		deltat = end - start;
	}

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), (autodif<T>)deltat);
		result(i).d.resize(n);
		result(i) = autodif<T>::expand(result(i), save);
	}

	init = result;

	psa< autodif<T> >::mode() = save_mode;
	psa< autodif<T> >::use_history() = save_uh;
	psa< autodif<T> >::record_history() = save_rh;
}

template <class T, class F>
void
odelong_nv(F f, ub::vector< autodif<T> >& init, const T& start, const T& end, ode_param<T> p = ode_param<T>()) {

	ub::vector< autodif<T> > x;
	T t, t1;

	x = init;
	t = start;
	p.set_autostep(true);
	while (1) {
		t1 = end;
		if (t == t1) break;

		ode_nv(f, x, t, t1, p);

		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x << "\n";
		}

		t = t1;
	}

	init = x;
}

} // namespace kv

#endif // ODE_AUTODIF_NV_HPP


#ifndef HOPS_MULTIVARIATEGAUSSIANMODEL_HPP
#define HOPS_MULTIVARIATEGAUSSIANMODEL_HPP

#define _USE_MATH_DEFINES
#include <math.h>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <utility>

namespace hops {
    template<typename Matrix, typename Vector>
    class MultivariateGaussianModel {
    public:
        using MatrixType = Matrix;
        using VectorType = Vector;

        MultivariateGaussianModel(VectorType mean, MatrixType covariance);

        /**
         * @brief Evaluates the negative log likelihood for input x.
         * @param x
         * @return
         */
        typename MatrixType::Scalar computeNegativeLogLikelihood(const VectorType &x) const;

        MatrixType computeExpectedFisherInformation(const VectorType &) const;

        VectorType computeLogLikelihoodGradient(const VectorType &x) const;

    private:
        VectorType mean;
        MatrixType covariance;
        MatrixType inverseCovariance;
        typename MatrixType::Scalar logNormalizationConstant;
    };

    template<typename MatrixType, typename VectorType>
    MultivariateGaussianModel<MatrixType, VectorType>::MultivariateGaussianModel(VectorType mean,
                                                                                MatrixType covariance) :
            mean(std::move(mean)),
            covariance(std::move(covariance)) {
        Eigen::LLT<MatrixType, Eigen::Upper> solver(this->covariance);
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixL = solver.matrixL();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixU = solver.matrixU();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> inverseMatrixL = matrixL.inverse();
        inverseCovariance = inverseMatrixL * inverseMatrixL.transpose();

        logNormalizationConstant = -static_cast<typename MatrixType::Scalar>(this->mean.rows()) / 2 *
                                   std::log(2 * M_PI)
                                   - matrixL.diagonal().array().log().sum();
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar
    MultivariateGaussianModel<MatrixType, VectorType>::computeNegativeLogLikelihood(const VectorType &x) const {
        return -logNormalizationConstant +
               0.5 * static_cast<typename MatrixType::Scalar>((x - mean).transpose() * inverseCovariance * (x - mean));
    }

    template<typename MatrixType, typename VectorType>
    MatrixType
    MultivariateGaussianModel<MatrixType, VectorType>::computeExpectedFisherInformation(const VectorType &) const {
        return inverseCovariance;
    }

    template<typename MatrixType, typename VectorType>
    VectorType
    MultivariateGaussianModel<MatrixType, VectorType>::computeLogLikelihoodGradient(const VectorType &x) const {
        return -inverseCovariance * (x - mean);
    }
}

#endif //HOPS_MULTIVARIATEGAUSSIANMODEL_HPP


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

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/triangulation.hpp>
#include <opencv2/sfm/projection.hpp>

// libmv headers
#include "libmv/multiview/twoviewtriangulation.h"
#include "libmv/multiview/fundamental.h"

using namespace cv;
using namespace std;

namespace cv
{
namespace sfm
{

/** @brief Triangulates the a 3d position between two 2d correspondences, using the DLT.
  @param xl Input vector with first 2d point.
  @param xr Input vector with second 2d point.
  @param Pl Input 3x4 first projection matrix.
  @param Pr Input 3x4 second projection matrix.
  @param objectPoint Output vector with computed 3d point.

  Reference: @cite HartleyZ00 12.2 pag.312
 */
void
triangulateDLT( const Vec2d &xl, const Vec2d &xr,
                const Matx34d &Pl, const Matx34d &Pr,
                Vec3d &point3d )
{
    Matx44d design;
    for (int i = 0; i < 4; ++i)
    {
        design(0,i) = xl(0) * Pl(2,i) - Pl(0,i);
        design(1,i) = xl(1) * Pl(2,i) - Pl(1,i);
        design(2,i) = xr(0) * Pr(2,i) - Pr(0,i);
        design(3,i) = xr(1) * Pr(2,i) - Pr(1,i);
    }

    Vec4d XHomogeneous;
    cv::SVD::solveZ(design, XHomogeneous);

    homogeneousToEuclidean(XHomogeneous, point3d);
}

/** @brief Triangulates the 3d position of 2d correspondences between n images, using the DLT
 * @param x Input vectors of 2d points (the inner vector is per image). Has to be 2xN
 * @param Ps Input vector with 3x4 projections matrices of each image.
 * @param X Output vector with computed 3d point.

 * Reference: it is the standard DLT; for derivation see appendix of Keir's thesis
 */
void
triangulateNViews(const Mat_<double> &x, const std::vector<Matx34d> &Ps, Vec3d &X)
{
    CV_Assert(x.rows == 2);
    unsigned nviews = x.cols;
    CV_Assert(nviews == Ps.size());

    cv::Mat_<double> design = cv::Mat_<double>::zeros(3*nviews, 4 + nviews);
    for (unsigned i=0; i < nviews; ++i) {
        for(char jj=0; jj<3; ++jj)
            for(char ii=0; ii<4; ++ii)
                design(3*i+jj, ii) = -Ps[i](jj, ii);
        design(3*i + 0, 4 + i) = x(0, i);
        design(3*i + 1, 4 + i) = x(1, i);
        design(3*i + 2, 4 + i) = 1.0;
    }

    Mat X_and_alphas;
    cv::SVD::solveZ(design, X_and_alphas);
    homogeneousToEuclidean(X_and_alphas.rowRange(0, 4), X);
}

void
triangulatePoints(InputArrayOfArrays _points2d, InputArrayOfArrays _projection_matrices,
                  OutputArray _points3d)
{
    // check
    size_t nviews = (unsigned) _points2d.total();
    CV_Assert(nviews >= 2 && nviews == _projection_matrices.total());

    // inputs
    size_t n_points;
    std::vector<Mat_<double> > points2d(nviews);
    std::vector<Matx34d> projection_matrices(nviews);
    {
        std::vector<Mat> points2d_tmp;
        _points2d.getMatVector(points2d_tmp);
        n_points = points2d_tmp[0].cols;

        std::vector<Mat> projection_matrices_tmp;
        _projection_matrices.getMatVector(projection_matrices_tmp);

        // Make sure the dimensions are right
        for(size_t i=0; i<nviews; ++i) {
            CV_Assert(points2d_tmp[i].rows == 2 && points2d_tmp[i].cols == n_points);
            if (points2d_tmp[i].type() == CV_64F)
                points2d[i] = points2d_tmp[i];
            else
                points2d_tmp[i].convertTo(points2d[i], CV_64F);

            CV_Assert(projection_matrices_tmp[i].rows == 3 && projection_matrices_tmp[i].cols == 4);
            if (projection_matrices_tmp[i].type() == CV_64F)
              projection_matrices[i] = projection_matrices_tmp[i];
            else
              projection_matrices_tmp[i].convertTo(projection_matrices[i], CV_64F);
        }
    }

    // output
    _points3d.create(3, n_points, CV_64F);
    cv::Mat points3d = _points3d.getMat();

    // Two view
    if( nviews == 2 )
    {
        const Mat_<double> &xl = points2d[0], &xr = points2d[1];

        const Matx34d & Pl = projection_matrices[0];    // left matrix projection
        const Matx34d & Pr = projection_matrices[1];    // right matrix projection

        // triangulate
        for( unsigned i = 0; i < n_points; ++i )
        {
            Vec3d point3d;
            triangulateDLT( Vec2d(xl(0,i), xl(1,i)), Vec2d(xr(0,i), xr(1,i)), Pl, Pr, point3d );
            for(char j=0; j<3; ++j)
                points3d.at<double>(j, i) = point3d[j];
        }
    }
    else if( nviews > 2 )
    {
        // triangulate
        for( unsigned i=0; i < n_points; ++i )
        {
            // build x matrix (one point per view)
            Mat_<double> x( 2, nviews );
            for( unsigned k=0; k < nviews; ++k )
            {
                points2d.at(k).col(i).copyTo( x.col(k) );
            }

            Vec3d point3d;
            triangulateNViews( x, projection_matrices, point3d );
            for(char j=0; j<3; ++j)
                points3d.at<double>(j, i) = point3d[j];
        }
    }
}

} /* namespace sfm */
} /* namespace cv */


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_ONLINESTAT_HPP
#define NETKET_ONLINESTAT_HPP

#include <mpi.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <iostream>
#include <vector>
#include "Utils/random_utils.hpp"

namespace netket {

/// Online statistics
// for general types
// this class accumulates results
// with minimal memory requirements
// simple statistics can then be obtained
// or merged with other bins

template <class T>
class OnlineStat {
  // Number of samples in this bin
  int N_;

  // current mean
  T mean_;

  // Running sum of squares of differences from the current mean
  T m2_;

  bool firstcall_;

 public:
  using DataType = T;

  explicit OnlineStat() { Reset(); }

  // Adding data to this bin
  inline void operator<<(const DataType &data) {
    CheckCall(data);

    N_ += 1;
    const T delta = data - mean_;
    mean_ += delta / double(N_);

    const T delta2 = data - mean_;
    m2_ += delta * delta2;
  }

  // Merging with another bin
  inline void operator<<(const OnlineStat<T> &obin) {
    CheckCall(obin.Mean());

    N_ += obin.N();
    const T delta = obin.Mean() - Mean();
    mean_ += delta * obin.N() / double(N_);

    m2_ += obin.m2_ + delta * delta * obin.N() * (1. - obin.N() / double(N_));
  }

  inline int N() const { return N_; }

  inline DataType Mean() const { return mean_; }

  inline DataType Variance() const { return m2_ / double(N_ - 1.); }

  inline DataType ErrorOfMean() const {
    return sqrt(m2_ / double(N_ * (N_ - 1.)));
  }

  void Reset() { firstcall_ = true; }

  void CheckCall(const DataType &data) {
    if (firstcall_) {
      N_ = 0;
      mean_ = DataType(data);
      mean_ = 0;
      m2_ = DataType(data);
      m2_ = 0;
      firstcall_ = false;
    }
  }
};

/// Online statistics
// for scalars
// this class accumulates results
// with minimal memory requirements
// simple statistics can then be obtained
// or merged with other bins
template <>
class OnlineStat<Eigen::VectorXd> {
  // Number of samples in this bin
  int N_;

  // current mean
  Eigen::VectorXd mean_;

  // Running sum of squares of differences from the current mean
  Eigen::VectorXd m2_;

  bool firstcall_;

 public:
  using DataType = Eigen::VectorXd;

  explicit OnlineStat() { Reset(); }

  // Adding data to this bin
  inline void operator<<(const DataType &data) {
    CheckCall(data);

    N_ += 1;
    const DataType delta = data - mean_;
    mean_ += delta / double(N_);

    const DataType delta2 = data - mean_;
    m2_ += delta.cwiseProduct(delta2);
  }

  // Merging with another bin
  inline void operator<<(const OnlineStat<Eigen::VectorXd> &obin) {
    CheckCall(obin.Mean());

    N_ += obin.N();
    const DataType delta = obin.Mean() - Mean();
    mean_ += delta * obin.N() / double(N_);
    m2_ += obin.m2_ +
           delta.cwiseProduct(delta) * obin.N() * (1. - obin.N() / double(N_));
  }

  inline int N() const { return N_; }

  inline DataType Mean() const { return mean_; }

  inline DataType Variance() const { return m2_ / double(N_ - 1.); }

  inline DataType ErrorOfMean() const {
    return m2_.cwiseSqrt() / std::sqrt(double(N_ * (N_ - 1.)));
  }

  void Reset() { firstcall_ = true; }

  void CheckCall(const DataType &data) {
    if (firstcall_) {
      N_ = 0;
      mean_.resize(data.size());
      mean_.setZero();
      m2_.resize(data.size());
      m2_.setZero();
      firstcall_ = false;
    }
  }
};

}  // namespace netket
#endif


/*
 * PoseOptimizationGeometric.cpp
 *
 *  Created on: Aug 30, 2017
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich
 */

#include <free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp>

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <kindr/Core>

using namespace Eigen;

namespace free_gait {

PoseOptimizationGeometric::PoseOptimizationGeometric(const AdapterBase& adapter)
    : PoseOptimizationBase(adapter)
{
}

PoseOptimizationGeometric::~PoseOptimizationGeometric()
{
}

void PoseOptimizationGeometric::setStanceForOrientation(const Stance& stance)
{
  stanceForOrientation_ = stance;
}

bool PoseOptimizationGeometric::optimize(Pose& pose)
{
  checkSupportRegion();

  // Planar position: Use geometric center of support region.
  Position center;
  center.vector().head(2) = supportRegion_.getCentroid();

  // Height: Average of stance plus default height.
  for (const auto& foot : stance_) {
    center.z() += foot.second.z() - nominalStanceInBaseFrame_.at(foot.first).z();
  }
  center.z() /= (double) stance_.size();
  pose.getPosition() = center;

  // Orientation: Squared error minimization (see sec. 4.2.2 from Bloesch, Technical
  // Implementations of the Sense of Balance, 2016).
  // Notes on (38):
  // - i: Identity pose (I),
  // - j: Solution (B),
  // - t = I_r_IB,
  // - q = q_IB,
  // - a_k = I_f_k: Foot position for leg k in inertial frame,
  // - b_k = B_\hat_f_K: Nominal foot position for leg k in base frame.

  Eigen::Matrix4d C(Eigen::Matrix4d::Zero()); // See (45).
  Eigen::Matrix4d A(Eigen::Matrix4d::Zero()); // See (46).
  for (const auto& foot : stance_) {
    kindr::QuaternionD footPositionInertialFrame(0.0, foot.second.vector()); // \bar_a_k = S^T * a_k (39).
    kindr::QuaternionD defaultFootPositionBaseFrame(0.0, nominalStanceInBaseFrame_.at(foot.first).vector()); // \bar_b_k = S^T * b_k.
    Eigen::Matrix4d Ak = footPositionInertialFrame.getQuaternionMatrix() - defaultFootPositionBaseFrame.getConjugateQuaternionMatrix(); // See (46).
    C += Ak * Ak; // See (45).
    A += Ak; // See (46).
  }
  A = A / ((double) stance_.size()); // Error in (46).
  C -= stance_.size() * A * A; // See (45).
  Eigen::EigenSolver<Eigen::Matrix4d> eigenSolver(C);
  int maxCoeff;
  eigenSolver.eigenvalues().real().maxCoeff(&maxCoeff);
  // Eigen vector corresponding to max. eigen value.
  pose.getRotation() = RotationQuaternion(eigenSolver.eigenvectors().col(maxCoeff).real());
  pose.getRotation().setUnique();

  // Get yaw rotation desired heading direction with respect to the target feet.
  const Position positionForeFeetMidPointInWorld = (stanceForOrientation_.at(LimbEnum::LF_LEG) + stanceForOrientation_.at(LimbEnum::RF_LEG)) * 0.5;
  const Position positionHindFeetMidPointInWorld = (stanceForOrientation_.at(LimbEnum::LH_LEG) + stanceForOrientation_.at(LimbEnum::RH_LEG)) * 0.5;
  Vector desiredHeadingDirectionInWorld = Vector(positionForeFeetMidPointInWorld - positionHindFeetMidPointInWorld);
  desiredHeadingDirectionInWorld.z() = 0.0;
  RotationQuaternion desiredHeading;
  desiredHeading.setFromVectors(Vector::UnitX().toImplementation(), desiredHeadingDirectionInWorld.vector());

  // Apply roll/pitch adaptation factor (~0.7).
  const RotationQuaternion yawRotation(RotationVector(RotationVector(pose.getRotation()).vector().cwiseProduct(Eigen::Vector3d::UnitZ())));
  const RotationQuaternion rollPitchRotation(RotationVector(0.7 * RotationVector(yawRotation.inverted() * pose.getRotation()).vector()));
//  pose.getRotation() = yawRotation * rollPitchRotation; // Alternative.
  pose.getRotation() = desiredHeading * rollPitchRotation;

  return true;
}

} /* namespace */


#include "jetEmission.h"

// Standard C++ libraries
#include <iomanip>

// Boost libraries
#include <boost/property_tree/ptree.hpp>
#include <boost/math/tools/roots.hpp>

// Standard user-made libraries
#include <fparameters/SpaceIterator.h>
#include <fparameters/parameters.h>
#include <fmath/physics.h>
#include <fparameters/Dimension.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_gamma.h>
#include "absorption.h"
// Project headers
#include "globalVariables.h"
#include "messages.h"
#include "read.h"
#include "adafFunctions.h"
#include "write.h"
#include <fmath/RungeKutta.h>

#include <fluminosities/thermalSync.h>
#include <fluminosities/blackBody.h>
#include "absorption.h"

double n_pl(double g, double gammaC, double gammaMin, double Npl, double pJet)
{
	if (gammaMin > gammaC) {
		double factor = 0.0;
		if (g > gammaC && g < gammaMin)
			factor = pow(gammaMin,1.0-pJet)/(g*g);
		else if (g > gammaMin)
			factor = pow(g,-(pJet+1.0));
		return Npl*(pJet-1.0)*gammaC*factor;
	} else {
		double factor = 0.0;
		if (g >= gammaMin && g <= gammaC)
			factor = 1.0;
		else if (g > gammaC)
			factor = gammaC/g;
		return Npl*(pJet-1.0)*factor*pow(g,-pJet);
	}
}

double n_pl_2(double g, double Npl, double pJet)
{
	return Npl*(pJet-1.0)*pow(g,-pJet);
}

double jSyJet(double g, double nu, double magf)
{
	double U_B = magf*magf/(8.0*pi);
	double nuB = electronCharge*magf/(2*pi*electronMass*cLight);
	double t = nu / (3.0*g*g*nuB);
	double K_43,K_13;
	if (t > 30) {
		K_43 = K_13 = exp(-t) * sqrt(0.5*pi/t);
	} else if (t < 0.01*sqrt(2)) {
		K_13 = 0.5*gsl_sf_gamma(1.0/3.0) * pow(2.0/t,1.0/3.0);
		K_43 = 0.5*gsl_sf_gamma(4.0/3.0) * pow(2.0/t,4.0/3.0);
	} else {
		K_43 = gsl_sf_bessel_Knu(4.0/3.0,t);
		K_13 = gsl_sf_bessel_Knu(1.0/3.0,t);
	}
	return 3.0*sqrt(3.0)/pi * thomson * cLight * U_B * t*t * (K_43*K_13-3*t/5 * (K_43*K_43-K_13*K_13));
}

double jSyJet2(double g, double nu, double magf)
{
	double nuC = 3.0/(4.0*pi)*electronCharge*magf/(electronMass*cLight) * g*g;
	double x = nu/nuC;
	return sqrt(3.0)*P3(electronCharge)/electronRestEnergy * magf *1.85*pow(x,1.0/3.0)*exp(-x);
}

double sigma_Sy(double g, double nu, double magf)
{
	double nuB = electronCharge*magf/(2*pi*electronMass*cLight);
	double t = nu/(3*g*g*nuB);
	double K_43,K_13;
	if (t > 30) {
		K_43 = K_13 = exp(-t) * sqrt(0.5*pi/t);
	} else if (t < 0.01*sqrt(2)) {
		K_13 = 0.5*gsl_sf_gamma(1.0/3.0) * pow(2.0/t,1.0/3.0);
		K_43 = 0.5*gsl_sf_gamma(4.0/3.0) * pow(2.0/t,4.0/3.0);
	} else {
		K_43 = gsl_sf_bessel_Knu(4.0/3.0,t);
		K_13 = gsl_sf_bessel_Knu(1.0/3.0,t);
	}
	return 8.0*sqrt(3.0)/15 *pi*pi*electronCharge / magf * t / pow(g,5.0) * (K_43*K_43-K_13*K_13);
}

double tau_Sy(double deltaR, double nu, double gammaC, double gammaMin, double Npl, double pJet, double magf)
{
	return deltaR * RungeKuttaSimple(gammaMin,1.0e10,[&nu,&gammaC,&gammaMin,&Npl,&pJet,&magf]
						(double g) {return sigma_Sy(g,nu,magf)*n_pl(g,gammaC,gammaMin,Npl,pJet);});
}

void jetProcesses(State& st, const string& filename)
{
	double accRateFraction = GlobalConfig.get<double>("nonThermal.jet.accRateFraction");
	double accRate = accRateFraction*accRateADAF(schwRadius);
	double openingAngle = GlobalConfig.get<double>("nonThermal.jet.openingAngle");
	double bulkLorentzFactor = GlobalConfig.get<double>("nonThermal.jet.lorentzFactor");
	double inclinationAngle = GlobalConfig.get<double>("nonThermal.jet.inclination")*(pi/180.0);
	double nt_electronFraction = GlobalConfig.get<double>("nonThermal.jet.etaInj");
	double eB = GlobalConfig.get<double>("nonThermal.jet.magneticEnergyFraction");
	double eElectrons = GlobalConfig.get<double>("nonThermal.jet.electronEnergyFraction");
	double pJet = GlobalConfig.get<double>("nonThermal.jet.pIndex");
	double zMin = GlobalConfig.get<double>("nonThermal.jet.zMin");

	double z0 = P2(bulkLorentzFactor)*schwRadius*zMin;
	double zMax = z0*1.0e4;
	size_t nZ = 100;

	double gamma2 = sqrt(0.5*(P2(bulkLorentzFactor)+1.0));
	double betaJet = sqrt(1.0-1.0/P2(bulkLorentzFactor));
	double vJet = betaJet*cLight;
	double n1 = accRate/(pi*z0*z0*openingAngle*openingAngle*vJet)/protonMass;
	double n2 = (4.0*gamma2+3.0)*n1;
	double Ti = 6.3e11;
	double Te = 1.0e9;

	double pasoZ = pow(zMax/z0,1.0/nZ);
	double z = z0;
	size_t jZ = 0;
	Matrix lumBeamed;
	matrixInit(lumBeamed,nE,nZ,0.0);
	while (z < zMax) {
		z *= pasoZ;

		double dz = z*(pasoZ-1.0);
		double area = pi*P2(openingAngle*z);
		double vol = area*dz;
		double dens = n2 * P2(z0/z);
		double magf = sqrt(8.0*pi*eB*(gamma2-1.0)*dens*protonMass*cLight2);

		// NONTHERMAL POPULATION
		double gammaMin = max((gamma2-1.0)*(pJet-2.0)/(pJet-1.0) * (protonMass/electronMass) *
								(eElectrons/nt_electronFraction),1.0);
		gammaMin = 2.0;
		double tDyn = z/cLight;
		double Npl = nt_electronFraction*dens*pow(gammaMin,pJet-1.0);
		double gammaC = 6*pi*electronRestEnergy/(magf*magf*thomson*z);

		size_t jE = 0;
		st.photon.ps.iterate([&](const SpaceIterator& i) {
			double nu = i.val(DIM_E)/planck;
			double beta = sqrt(1.0-1.0/P2(bulkLorentzFactor));
			double doppler = 1.0/(bulkLorentzFactor*(1.0-beta*cos(inclinationAngle)));
			double nuDoppler = nu/doppler;

			double lumLocal = 0.0;
			double gamma1 = 2.0;
			double gammaMax = 1.0e7;
			size_t nnE = 1000;
			double pasoG = pow(gammaMax/gamma1,1.0/nnE);
			double gamma = gamma1;
			for (size_t jjE=0;jjE<nnE;jjE++) {
				double dgamma = gamma*(pasoG-1.0);
				//if (jE==0)
				//	cout << z/schwRadius << "\t" << gamma << "\t" << n_pl(gamma,gammaC,gammaMin,Npl,pJet)*dgamma << endl;
				lumLocal += dgamma*n_pl(gamma,gammaC,gammaMin,Npl,pJet)*jSyJet2(gamma,nuDoppler,magf);
				//lumLocal += dgamma*n_pl_2(gamma,Npl,pJet)*jSyJet2(gamma,nuDoppler,magf);
				gamma *= pasoG;
			}
			lumLocal *= vol;

			//double lumLocal = vol*RungeKuttaSimple(gammaC,gammaC*1.0e5,[&gammaC,&gammaMin,&Npl,
			//					&pJet,&magf,&nuDoppler](double g){return n_pl(g,gammaC,gammaMin,Npl,pJet)*
			//					jSyJet2(g,nuDoppler,magf);});

			double a_th = jSync(planck*nu,Te,magf,dens)/bb(nu,Te);
			double a_pl = RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&](double g)
						{return n_pl(g,gammaC,gammaMin,Npl,pJet)*sigma_Sy(g,nu,magf);});
			double tau = z*openingAngle*(a_pl+a_th);
			//tau = tau_Sy(z*openingAngle,nu,gammaC,gammaMin,Npl,pJet,magf);
			lumLocal *= (tau > 1.0e-10 ? (1.0-exp(-tau))/tau : 1.0);
			lumBeamed[jE][jZ] = lumLocal * doppler*doppler*doppler;
			jE++;
		},{-1,0,0});
		jZ++;
	}

	ofstream file;
	file.open(filename.c_str(),ios::out);
	size_t jE = 0.0;
	st.photon.ps.iterate([&](const SpaceIterator& i) {
		double nu = i.val(DIM_E)/planck;
		double lum = 0.0;
		for (size_t jZ=0;jZ<nZ;jZ++) {
			lum += lumBeamed[jE][jZ];
		}
		file << nu << "\t" << nu*lum << endl;
		jE++;
	},{-1,0,0});
	file.close();
}

void jetProcesses2(State& st, const string& filename)
{
	double accRateFraction = GlobalConfig.get<double>("nonThermal.jet.accRateFraction");
	double accRate = accRateFraction*accRateADAF(schwRadius);
	double openingAngle = GlobalConfig.get<double>("nonThermal.jet.openingAngle");
	double bulkLorentzFactor = GlobalConfig.get<double>("nonThermal.jet.lorentzFactor");
	double inclinationAngle = GlobalConfig.get<double>("nonThermal.jet.inclination")*(pi/180.0);
	double nt_electronFraction = GlobalConfig.get<double>("nonThermal.jet.etaInj");
	double pJet = GlobalConfig.get<double>("nonThermal.jet.pIndex");
	double mB = 1.5;
	size_t nZ = 100;

	double z0 = 50*schwRadius;
	double r0 = openingAngle*z0;
	double betaJet = sqrt(1.0-1.0/P2(bulkLorentzFactor));
	double vJet = betaJet*cLight;
	double B0 = sqrt(accRate*cLight2*8/(r0*r0*vJet));
	double n0 = accRate/(bulkLorentzFactor*pi*r0*r0*vJet*protonMass);
	double zMax = z0*1.0e5;

	double pasoZ = pow(zMax/z0,1.0/nZ);
	double z = z0;
	size_t jZ = 0;
	Matrix lumBeamed;
	matrixInit(lumBeamed,nE,nZ,0.0);
	while (z < zMax) {
		z *= pasoZ;
		double dz = z*(pasoZ-1.0);
		double area = pi*P2(openingAngle*z);
		double vol = area*dz;
		double dens = n0 * P2(z0/z);
		double magf = B0 * pow(z0/z,mB);

		// NONTHERMAL POPULATION
		double gammaMin = 2.0;
		double Npl = nt_electronFraction*dens*pow(gammaMin,pJet-1.0);
		double gammaC = 6*pi*electronRestEnergy/(magf*magf*thomson*z);

		size_t jE = 0;
		st.photon.ps.iterate([&](const SpaceIterator& i) {
			double nu = i.val(DIM_E)/planck;
			double beta = sqrt(1.0-1.0/P2(bulkLorentzFactor));
			double doppler = 1.0/(bulkLorentzFactor*(1.0-beta*cos(inclinationAngle)));
			double nuDoppler = nu/doppler;
			double lumLocal = vol*RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&gammaC,&gammaMin,&Npl,
									&pJet,&magf,&nuDoppler](double g){return n_pl(g,gammaC,gammaMin,Npl,pJet)*
										jSyJet2(g,nuDoppler,magf);});
			double a_pl = RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&](double g)
							{return n_pl(g,gammaC,gammaMin,Npl,pJet)*sigma_Sy(g,nu,magf);});
			double tau = z*openingAngle*a_pl;
			//tau = tau_Sy(z*openingAngle,nu,gammaC,gammaMin,Npl,pJet,magf);
			lumLocal *= (tau > 1.0e-10 ? (1.0-exp(-tau))/tau : 1.0);
			lumBeamed[jE][jZ] = lumLocal * doppler*doppler*doppler;
			jE++;
		},{-1,0,0});
		jZ++;
	}

	ofstream file;
	file.open(filename.c_str(),ios::out);
	size_t jE = 0.0;
	st.photon.ps.iterate([&](const SpaceIterator& i) {
		double nu = i.val(DIM_E)/planck;
		double lum = 0.0;
		for (size_t jZ=0;jZ<nZ;jZ++) {
			lum += lumBeamed[jE][jZ];
		}
		file << nu << "\t" << nu*lum << endl;
		jE++;
	},{-1,0,0});
	file.close();
}


// BLAS level 3
// hermitian matrices, her2k

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;
namespace traits = boost::numeric::bindings::traits;

using std::cout;
using std::cin;
using std::endl;

typedef float real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> ccm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> crm_t;
typedef ublas::hermitian_adaptor<ccm_t, ublas::upper> cucha_t;
typedef ublas::hermitian_adaptor<ccm_t, ublas::lower> clcha_t;
typedef ublas::hermitian_adaptor<crm_t, ublas::upper> curha_t;
typedef ublas::hermitian_adaptor<crm_t, ublas::lower> clrha_t;

int main() {

  // complex

  const int n1 = 3;
  const int k1 = 2;

  ccm_t cac (n1, k1);
  crm_t car (n1, k1);
  cac(0,0) = car(0,0) = cmplx_t (1., 1.);
  cac(1,0) = car(1,0) = cmplx_t (2., 1.);
  cac(2,0) = car(2,0) = cmplx_t (3., 1.);
  cac(0,1) = car(0,1) = cmplx_t (1., 1.);
  cac(1,1) = car(1,1) = cmplx_t (2., 1.);
  cac(2,1) = car(2,1) = cmplx_t (3., 1.);
  print_m (cac, "cac");
  cout << endl;
  print_m (car, "car");
  cout << endl << endl;

  ccm_t cbc (n1, k1);
  crm_t cbr (n1, k1);
  cbc(0,0) = cbr(0,0) = cmplx_t (0., -1.);
  cbc(1,0) = cbr(1,0) = cmplx_t (0., -1.);
  cbc(2,0) = cbr(2,0) = cmplx_t (0., -1.);
  cbc(0,1) = cbr(0,1) = cmplx_t (0., -1.);
  cbc(1,1) = cbr(1,1) = cmplx_t (0., -1.);
  cbc(2,1) = cbr(2,1) = cmplx_t (0., -1.);
  print_m (cbc, "cbc");
  cout << endl;
  print_m (cbr, "cbr");
  cout << endl << endl;

  ccm_t ccmu (n1, n1);
  ccm_t ccml (n1, n1);
  crm_t crmu (n1, n1);
  crm_t crml (n1, n1);
  cucha_t cucha (ccmu);
  clcha_t clcha (ccml);
  curha_t curha (crmu);
  clrha_t clrha (crml);

  atlas::her2k (CblasNoTrans, cac, cbc, cucha);
  atlas::her2k (CblasNoTrans, cmplx_t(1,0), cac, cbc, 0., clcha);
  atlas::her2k (CblasNoTrans, cmplx_t(1,0), car, cbr, 0., curha);
  atlas::her2k (CblasNoTrans, car, cbr, clrha);

  print_m (cucha, "cucha");
  cout << endl;
  print_m (clcha, "clcha");
  cout << endl;
  print_m (curha, "curha");
  cout << endl;
  print_m (clrha, "clrha");
  cout << endl << endl;

  // part 2

  ccm_t cact (ublas::herm (cac));
  crm_t cart (ublas::herm (car));
  print_m (cact, "cact");
  cout << endl;
  print_m (cart, "cart");
  cout << endl << endl;

  ccm_t cbct (ublas::herm (cbc));
  crm_t cbrt (ublas::herm (cbr));
  print_m (cbct, "cbct");
  cout << endl;
  print_m (cbrt, "cbrt");
  cout << endl << endl;

  init_m (ccmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (ccml, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (crmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (crml, const_val<cmplx_t> (cmplx_t (0, 0)));

  atlas::her2k (CblasUpper, CblasConjTrans, 1.0, cact, cbct, 0.0, ccmu);
  atlas::her2k (CblasLower, CblasConjTrans, 1.0, cact, cbct, 0.0, ccml);
  atlas::her2k (CblasUpper, CblasConjTrans, 1.0, cart, cbrt, 0.0, crmu);
  atlas::her2k (CblasLower, CblasConjTrans, 1.0, cart, cbrt, 0.0, crml);

  print_m (ccmu, "ccmu");
  cout << endl;
  print_m (ccml, "ccml");
  cout << endl;
  print_m (crmu, "crmu");
  cout << endl;
  print_m (crml, "crml");
  cout << endl;
}


#include "timer.h"
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>

using numeric_t = double;

numeric_t flushCache() {
    const size_t N = 1000;
    const Eigen::MatrixXd A = Eigen::MatrixXd::Random(N, N);
    const Eigen::VectorXd x = Eigen::MatrixXd::Random(N, 1);

    return x.transpose() * A * x;
}

int main() {
    const numeric_t alpha = 1e-4;
    const size_t L = 1000;
    const size_t T = 1;
    const size_t N = 1000000;
    const numeric_t tau = 0.001;

    assert(tau < std::pow((L / static_cast<numeric_t>(N)), 2) / (2. * alpha));

    auto u0 = [](numeric_t x) {
        return std::sin(2. * M_PI * x / L);
    };
    Eigen::MatrixXd U = Eigen::MatrixXd::Zero(N, T / tau);

    Timer timer;

    for (size_t lap = 0; lap < 10; ++lap) {
        std::cout << "starting lap: " << lap << std::endl;
        timer.start();

        const Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N, 0, L);
        U.col(0) = x.unaryExpr(u0);

        for (size_t t = 0; t < ((T / tau) - 1); ++t) {
            for (size_t i = 0; i < N; ++i) {
                U(i, t + 1) = U(i, t) + (tau * alpha / (x(1) * x(1))) * (
                        U(((i - 1) + N) % N, t) - 2. * U(i, t) +
                        U((i + 1) % N, t)
                );
            }
        }

        timer.lap();

        std::cout << "flushing cache: " << flushCache() << std::endl;
    }

    std::cout << "Average runtime: " << timer.mean() << "s" << std::endl;
    std::cout << "Minimal runtime: " << timer.min() << "s" << std::endl;

    return 0;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/transform.hpp>
#include <boost/metaparse/always.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;
using boost::metaparse::transform;
using boost::metaparse::always;

using boost::mpl::apply_wrap1;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::if_;
using boost::mpl::bool_;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= value_exp ((mult_token | div_token) value_exp)*
 * value_exp ::= int_token | '_'
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;
typedef token<lit_c<'_'> > arg_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct build_plus
{
  template <class A, class B>
  class _plus
  {
  public:
    typedef _plus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) + _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _minus
  {
  public:
    typedef _minus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) - _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '+'>::type,
      _plus<State, typename back<C>::type>,
      _minus<State, typename back<C>::type>
    >
  {};
};

struct build_mult
{
  template <class A, class B>
  class _mult
  {
  public:
    typedef _mult type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) * _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _div
  {
  public:
    typedef _div type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) / _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '*'>::type,
      _mult<State, typename back<C>::type>,
      _div<State, typename back<C>::type>
    >
  {};
};

struct build_value
{
  typedef build_value type;

  template <class V>
  struct apply
  {
    typedef apply type;

    template <class T>
    int operator()(T) const
    {
      return V::type::value;
    }
  };
};

struct arg
{
  typedef arg type;

  template <class T>
  T operator()(T t) const
  {
    return t;
  }
};

typedef
  one_of<transform<int_token, build_value>, always<arg_token, arg> >
  value_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, value_exp>,
    value_exp,
    build_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    build_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > function_parser;

#if BOOST_METAPARSE_STD < 2011

template <class Exp>
struct lambda : apply_wrap1<function_parser, Exp> {};

using boost::metaparse::string;

lambda<string<'1','3'> >::type f1;
lambda<string<'2',' ','+',' ','3'> >::type f2;
lambda<string<'2',' ','*',' ','2'> >::type f3;
lambda<string<' ','1','+',' ','2','*','4','-','6','/','2'> >::type f4;
lambda<string<'2',' ','*',' ','_'> >::type f5;

#else

#ifdef LAMBDA
  #error LAMBDA already defined
#endif
#define LAMBDA(exp) apply_wrap1<function_parser, BOOST_METAPARSE_STRING(#exp)>::type

LAMBDA(13) f1;
LAMBDA(2 + 3) f2;
LAMBDA(2 * 2) f3;
LAMBDA( 1+ 2*4-6/2) f4;
LAMBDA(2 * _) f5;

#endif

int main()
{
  using std::cout;
  using std::endl;

  cout
    << f1(11) << endl
    << f2(11) << endl
    << f3(11) << endl
    << f4(11) << endl
    << f5(11) << endl
    << f5(1.1) << endl
    ;
}


// MIT License
//
// Copyright (c) 2018 Michal Siedlaczek
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

/// \file
/// \author Michal Siedlaczek
/// \copyright MIT License

#pragma once

#include <algorithm>
#include <cmath>
#include <functional>
#include <numeric>
#include <vector>

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

namespace taily {

struct Feature_Statistics {
    static constexpr std::size_t struct_size = 2 * sizeof(double) + sizeof(int);
    double expected_value;
    double variance;
    std::int64_t frequency;

    [[nodiscard]] constexpr auto
    operator+(Feature_Statistics const& other) const -> Feature_Statistics
    {
        return Feature_Statistics{expected_value + other.expected_value,
                                  variance + other.variance,
                                  frequency + other.frequency};
    }

    auto to_stream(std::ostream& os) const -> std::ostream&
    {
        os.write(reinterpret_cast<const char*>(&expected_value), sizeof(expected_value));
        os.write(reinterpret_cast<const char*>(&variance), sizeof(variance));
        os.write(reinterpret_cast<const char*>(&frequency), sizeof(frequency));
        return os;
    }

    [[nodiscard]] static auto from_stream(std::istream& is) -> Feature_Statistics
    {
        Feature_Statistics stats;
        is.read(reinterpret_cast<char*>(&stats.expected_value), sizeof(stats.expected_value));
        is.read(reinterpret_cast<char*>(&stats.variance), sizeof(stats.variance));
        is.read(reinterpret_cast<char*>(&stats.frequency), sizeof(stats.frequency));
        return stats;
    }

    template<typename Feature_Range>
    [[nodiscard]] static constexpr auto
    from_features(Feature_Range const& features) -> Feature_Statistics
    {
        return from_features(std::begin(features), std::end(features));
    }

    template<typename Forward_Iterator>
    [[nodiscard]] static constexpr auto
    from_features(Forward_Iterator first, Forward_Iterator last) -> Feature_Statistics
    {
        if (first == last) return Feature_Statistics{0, 0, 0};
        std::int64_t count{0};
        auto accumulate_feature = [&count](double const& acc, double const& feature) {
            count += 1;
            return acc + feature;
        };
        double const sum = std::accumulate(first, last, double{0.0}, accumulate_feature);
        double const expected_value = sum / count;
        auto accumulate_squared = [&expected_value](double const& acc, double const& feature) {
            return acc + std::pow(expected_value - feature, 2.0);
        };
        double const variance = std::accumulate(first, last, double{0.0}, accumulate_squared)
            / count;
        return Feature_Statistics{expected_value, variance, count};
    }
};

struct Query_Statistics {
    std::vector<Feature_Statistics> term_stats;
    std::int64_t collection_size;
};

/// Extimates the number of documents containing **any** of the terms
/// represented by `term_stats` in a collection of size `collection_size`.
[[nodiscard]] auto any(Query_Statistics const& stats) -> double
{
    const auto collection_size = stats.collection_size;
    const double any_product = std::accumulate(
        stats.term_stats.begin(),
        stats.term_stats.end(),
        1.0,
        [collection_size](const auto& acc, const Feature_Statistics& stats) {
            return acc * (1.0 - double(stats.frequency) / collection_size);
        });
    return collection_size * (1.0 - any_product);
}

/// Extimates the number of documents containing **all** of the terms
/// represented by `term_stats` in a collection of size `collection_size`.
[[nodiscard]] auto all(const Query_Statistics& stats) -> double
{
    double const any = taily::any(stats);
    if (any == 0.0) {
        return 0.0;
    }
    double const all_product = std::accumulate(
        stats.term_stats.begin(),
        stats.term_stats.end(),
        1.0,
        [any](auto const& acc, Feature_Statistics const& stats) {
            return acc * (stats.frequency / any);
        });
    return any * all_product;
}

/// Returns a gamma distribution fitted to `term_stats`.
[[nodiscard]] auto
fit_distribution(Feature_Statistics const& query_term_stats) -> boost::math::gamma_distribution<>
{
    const double k = std::pow(query_term_stats.expected_value, 2.0)
        / query_term_stats.variance;
    const double theta =
        query_term_stats.variance / query_term_stats.expected_value;
    return boost::math::gamma_distribution<>(k, theta);
}

/// Returns a gamma distribution fitted to a vector of term stats.
///
/// The term statictics are accumulated before the distribution is fitted.
[[nodiscard]] auto fit_distribution(std::vector<Feature_Statistics> const& term_stats)
    -> boost::math::gamma_distribution<>
{
    Feature_Statistics query_stats = std::accumulate(
        term_stats.begin(), term_stats.end(), Feature_Statistics{0, 0, 0});
    return fit_distribution(query_stats);
}

/// Estimates the global cutoff score for the entire collection.
[[nodiscard]] auto estimate_cutoff(Query_Statistics const& stats, int ntop) -> double
{
    auto const dist = fit_distribution(stats.term_stats);
    double const all = taily::all(stats);
    double const p_c = std::min(1.0, ntop / all);
    return boost::math::quantile(complement(dist, p_c));
}

/// Calculates the probability that a document in a shard given by `stats`
/// has a score higher than `cutoff`.
[[nodiscard]] auto calculate_cdf(double const cutoff, Query_Statistics const& stats) -> double
// [[expects: cutoff >= 0.0]]
{
    if (cutoff <= 0) {
        return 1.0;
    }
    Feature_Statistics query_stats = std::accumulate(
        stats.term_stats.begin(), stats.term_stats.end(), Feature_Statistics{0, 0, 0});
    if (query_stats.expected_value == 0 || query_stats.variance == 0) {
        return 0.0;
    }
    auto dist = fit_distribution(query_stats);
    return boost::math::cdf(complement(dist, cutoff));
}

/// Scores shards given by `shard_stats`.
///
/// \param global_stats Term statistics for the entire collection
/// \param shard_stats Term statistics for individual shards
/// \param ntop The parameter to Taily algorithm saying how many top results we
/// are shooting for
[[nodiscard]] auto score_shards(Query_Statistics const& global_stats,
                                std::vector<Query_Statistics> const& shard_stats,
                                int const ntop) -> std::vector<double>
{
    int const shard_count = shard_stats.size();

    std::vector<double> shard_all(shard_count);
    std::transform(std::begin(shard_stats),
                   std::end(shard_stats),
                   std::begin(shard_all),
                   [](auto const& shard_stats) { return taily::all(shard_stats); });

    double const global_all = all(global_stats);
    double const global_cutoff = estimate_cutoff(global_stats, ntop);

    std::vector<double> shard_coefs(shard_count);
    std::transform(std::begin(shard_stats),
                   std::end(shard_stats),
                   std::begin(shard_coefs),
                   std::bind(calculate_cdf, global_cutoff, std::placeholders::_1));

    std::transform(std::begin(shard_coefs),
                   std::end(shard_coefs),
                   std::begin(shard_all),
                   std::begin(shard_coefs),
                   std::multiplies<double>());

    double const normalization_factor = std::accumulate(
        std::begin(shard_coefs), std::end(shard_coefs), 0.0);

    std::vector<double> estimates(shard_count);
    auto normalize = [ntop, normalization_factor](auto const& element) {
        return normalization_factor > 0 ? element * ntop / normalization_factor : 0.0;
    };
    std::transform(
        std::begin(shard_coefs), std::end(shard_coefs), std::begin(estimates), normalize);
    return estimates;
}

}  // namespace taily


#include "include_and_types.cpp"
#include <set>
#include <boost/graph/detail/set_adaptor.hpp>

typedef graph_traits<Graph>::adjacency_iterator adj_iter;

template <class Root, class InComponent, class Num, class Set>
class TransitiveClosure{
private:
    //Graph
    Graph*g;

    //Property Map instances
    Root root;
    InComponent inComp;
    Num num;
    Set succ;

    int index;

    //Data Structure feeding Property Map instances
    std::vector<Vertex> rootVet;
    std::vector<bool> inCompVet;
    std::vector<std::set<Vertex>*> sets;
    std::vector<int> numVet;
    std::vector<std::set<Vertex>> vect;
    std::stack<Vertex> s;

    /**
     * This function is the body of the algorithm. It takes the vertex that the procedure is visiting in that time
     * and calculates its successors.
     * This is the C++ implementation of the pseudocode available on the Nuutila's paper
     * @param v, vertex visited
     */
    void tc(Vertex v){
        ++index;
        put(root, v, v);
        put(inComp, v, false);
        put(num, v, index);
        put(succ,v,&vect[v]);
        std::set<Vertex> roots;
        adj_iter ai, a_end;
        for(boost::tie(ai, a_end)= adjacent_vertices(v,*g); ai!=a_end;++ai){
            Vertex w=*ai;
            if(get(num,w)==0)//Checking if w is already visited
                tc(w);
            if(!get(inComp,get(root,w)))//Checking if w's root is already in a SCC
                put(root, v, get(num, get(root,w))<get(num,get(root,v)) ? get(root,w) : get(root,v));
            else
                //complexity logaritmic in size
                roots.insert(get(root,w));
        }
        std::set<Vertex>::iterator it;
        //complexity of set_union: 2*(size_set1+size_set2)-1
        //This loop inserts all the vertex in roots and their successors in v's root successors
        for(it=roots.begin();it!=roots.end();++it){
            Vertex r=*it;
            std::set<Vertex>* succRoot=get(succ, get(root,v));
            set_union(get(succ,r)->begin(),get(succ,r)->end(),succRoot->begin(), succRoot->end(),
                      std::inserter(*succRoot,succRoot->begin()));
            succRoot->insert(r);
        }

        if(get(root,v)==v){
            if(get(num,s.top())>=get(num,v)){
                get(succ,v)->insert(v);
                do{
                    Vertex w=s.top();
                    s.pop();
                    put(inComp,w,true);
                    if(v!=w){
                        //Pointer assignment, not a copy
                        std::set_union(get(succ,w)->begin(),get(succ,w)->end(),get(succ,v)->begin(),
                                       get(succ,v)->end(),std::inserter(*get(succ,v),get(succ,v)->begin()));
                        put(succ,w, get(succ,v));
                    }
                }while(get(num,s.top())>=get(num,v));
            }else{
                //if a vertex is root of a trivial SCC
                put(inComp,v,true);
            }
        }else{
            //If a vertex is not a root, it pushes his root into the stack, if it is not
            if(get(num,s.top())!=get(num, get(root,v)))
                s.push(get(root,v));
            get(succ,get(root,v))->insert(v);

        }
    }

    /**
     * This function launches the procedure and it performs the visit of all the graph's vertex
     */
    void transitive_closure_main(){
        vertex_iter vi,v_end;
        timer::auto_cpu_timer t;
        for(boost::tie(vi,v_end)=vertices(*g);vi!=v_end;++vi){
            Vertex v=*vi;
            if(get(num,v)==0){
                tc(v);
            }
        }
    };

public:
    /**
     * This is the constructor of the TransitiveClosure class. It takes the reference of the already allocated graph
     * done by the caller (main_transitive_closure.cpp).
     * The constructor initializes all the class attributes in proper way
     * @param graph pointer
     */
    TransitiveClosure(Graph*graph){
        g=graph;

        //Insertion and handling of a new vertex with lowest num possible in order to accomplish algorithm correctness
        Vertex starter=add_vertex(*g);
        int n=num_vertices(*g);
        numVet=*new std::vector<int>(n);
        num=make_iterator_property_map(numVet.begin(),get(vertex_index,*g));
        put(num, starter, -1);
        s.push(starter);
        remove_vertex(starter,*g);

        //Updating number of graph's vertices
        n--;

        //data structures allocation
        rootVet.resize(n);
        inCompVet.resize(n);
        sets.resize(n);
        vect.resize(n);

        //Properties instantiation
        root=make_iterator_property_map(rootVet.begin(), get(vertex_index,*g));
        inComp= make_iterator_property_map(inCompVet.begin(), get(vertex_index,*g));
        succ=make_iterator_property_map(sets.begin(),get(vertex_index,*g));

        //index needed
        index=0;
    }

    /**
     * This function is called by the main_transitive_closure callee. It launches the procedure and prints the
     * procedure's result when it is done.
     */
    void transitive_closure_scc(){
        //Execution of the procedure
        transitive_closure_main();

        //Printing our results
        std::cout << "Ours" << std::endl;
        IndexMap index = get(vertex_index,*g);
        vertex_iter it, it_end;
        for(boost::tie(it,it_end)=vertices(*g);it!=it_end;++it){
            Vertex v=*it;
            std::set<Vertex>* set=get(succ,get(root,v));
            std::cout << index[*it] << " --> ";
            std::set<Vertex>::iterator i;
            for(i=set->begin();i!=set->end();++i){
                std::cout << *i << " ";
            }
            std::cout << std::endl;
        }

    }
};