#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


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector3d v(1,0,0);
Vector3d w(1e-4,0,1);
cout << "Here's the vector v:" << endl << v << endl;
cout << "Here's the vector w:" << endl << w << endl;
cout << "v.isOrthogonal(w) returns: " << v.isOrthogonal(w) << endl;
cout << "v.isOrthogonal(w,1e-3) returns: " << v.isOrthogonal(w,1e-3) << endl;

  return 0;
}


// Filthy awful hack to test.
#include "gtest/gtest.h"
#include <Eigen/Dense>
#define private public
#include "bond.h"

TEST(ProjectionTest, OnRailUnchanged)
{
    // Test if projecting a vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << 1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, UnnormalisedRail)
{
    // Test if projecting a vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 10.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << 1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, OnRailOpposite)
{
    // Test if projecting negative vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << -1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, OnRailLarger)
{
    // Test if projecting a larger vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << 2.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, OnRailOppositeLarger)
{
    // Test if projecting a larger negative vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << -2.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, OffRail)
{
    // Test if projecting a vector that's off a rail back onto the rail
    // changes it.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << 1.0, 1.0;

    Eigen::VectorXd expected_vec(2);
    expected_vec << 1.0, 0.0;
    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), expected_vec(i));
    }
}

TEST(ProjectionTest, OffRailNegative)
{
    // Test if projecting a vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << 1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << -1.0, -1.0;

    Eigen::VectorXd expected_vec(2);
    expected_vec << -1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);
    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), expected_vec(i));
    }
}

TEST(ProjectionTest, NegativeRail)
{
    // Test if projecting negative vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << -1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << -1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}

TEST(ProjectionTest, NegativeRailOppositeVec)
{
    // Test if projecting negative vector onto itself leaves the vector unchanged.
    Eigen::VectorXd rail(2);
    rail << -1.0, 0.0;
    const HarmonicBond bond {1.0, 1.0, 0, 1, rail};

    Eigen::VectorXd new_vec(2);
    new_vec << 1.0, 0.0;

    auto projected_vec = bond.project_onto_rail(new_vec);

    for (int i = 0; i < new_vec.rows(); ++i) {
        ASSERT_FLOAT_EQ(projected_vec(i), new_vec(i));
    }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <qlo/latentmodels.hpp>

#include <ql/experimental/credit/defaultprobabilitylatentmodel.hpp>

namespace QuantLibAddin {

    GaussianDefProbLM::GaussianDefProbLM(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const boost::shared_ptr<QuantLib::Basket>& basket,
        const std::vector<std::vector<QuantLib::Real> >& factorWeights,
        bool permanent)
    : ObjectHandler::LibraryObject<QuantLib::GaussianDefProbLM>(properties,
      permanent) {
        libraryObject_ = boost::make_shared<QuantLib::GaussianDefProbLM>(
            //basket,
            factorWeights,
            QuantLib::LatentModelIntegrationType::GaussianQuadrature);
        libraryObject_->resetBasket(basket);
    }

    TDefProbLM::TDefProbLM(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const std::vector<QuantLib::Integer>& tOrders,
        const boost::shared_ptr<QuantLib::Basket>& basket,
        const std::vector<std::vector<QuantLib::Real> >& factorWeights,
        bool permanent)
    : ObjectHandler::LibraryObject<QuantLib::TDefProbLM>(properties,
      permanent) {
        QuantLib::TCopulaPolicy::initTraits initsT;
        initsT.tOrders = tOrders;
        libraryObject_ = boost::make_shared<QuantLib::TDefProbLM>(
            //basket,
            factorWeights,
            QuantLib::LatentModelIntegrationType::GaussianQuadrature,
            initsT);
        libraryObject_->resetBasket(basket);
    }

}


/**
 * @file NlpDescription.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-06
 */

#pragma once

#include <iostream>
#include <Eigen/Dense>

namespace solver
{

  // Nonlinear problem description
  class NlpDescription
  {
    public:
	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  NlpDescription(){};
      virtual ~NlpDescription(){};

      // definition of problem size
      virtual void getNlpParameters(int& n_vars, int& n_cons) = 0;

      // definition of problem box constraints
      virtual void getNlpBounds(int n_vars, int n_cons, double* x_l, double* x_u, double* g_l, double* g_u) = 0;

      // definition of starting point
      virtual void getStartingPoint(int n_vars, double* x) = 0;

      // definition of objective function
      virtual double evaluateObjective(int n_vars, const double* x) = 0;

      // definition of constraints function
      virtual void evaluateConstraintsVector(int n_vars, int n_cons, const double* x, double* constraints) = 0;

      // definition of how to process solution
      virtual void processSolution(int n_vars, double objective_value, const double* x)
      {
        Eigen::Map<const Eigen::VectorXd> eig_x_const(&x[0], n_vars);
        this->optimalVector() = eig_x_const;
        this->optimalValue()  = objective_value;
      }

      double& optimalValue() { return eig_obj_; }
      const double& optimalValue() const { return eig_obj_; }
      Eigen::VectorXd& optimalVector() { return eig_opt_x_; }
      const Eigen::VectorXd& optimalVector() const { return eig_opt_x_; }

    private:
      double eig_obj_;
      Eigen::VectorXd eig_opt_x_;
  };
}


/*
 * PACDDecrypter.cpp
 *
 *  Created on: 26 Jun 2018
 *      Author: scsjd
 */

#include "PolyACDDecrypter.h"

#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/vec_ZZ.h>
#include <jsoncpp/json/json.h>
#include <sstream>

PolyACDDecrypter::PolyACDDecrypter() {
}

PolyACDDecrypter::~PolyACDDecrypter() {
}

void PolyACDDecrypter::readSecretsFromJSON(std::string& json){
	Json::Value root;   // will contains the root value after parsing.
	Json::Reader reader;
	bool parsingSuccessful = reader.parse(json,root);
	if (parsingSuccessful){
		std::istringstream k1buf(root["k1"].asString());
		k1buf >> k1;
		std::istringstream k2buf(root["k2"].asString());
		k2buf >> k2;
	}
};

NTL::ZZX PolyACDDecrypter::decrypt(NTL::ZZX& ciphertext){
	NTL::ZZ c_l = LeadCoeff(ciphertext);
	NTL::ZZ m_n = c_l/k1;
	NTL::ZZX c;
	long d = deg(ciphertext)+1;
	c.SetLength(d);
	for (long i=0; i<d-1; i++){
		SetCoeff(c,i,coeff(ciphertext,i));
	}
	SetCoeff(c,d,m_n);
	SetCoeff(c,d-1,c_l-k1*m_n);
	NTL::ZZX m = c/k2;
	return m;
};


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


// This comes from the following Boost example:
// https://www.boost.org/doc/libs/1_71_0/doc/html/boost_random/tutorial.html
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <iostream>

int main() {
    std::string chars(
        "abcdefghijklmnopqrstuvwxyz"
        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
        "1234567890"
        "!@#$%^&*()"
        "`~-_=+[{]}\\|;:'\",<.>/? ");
    boost::random::random_device rng;
    boost::random::uniform_int_distribution<> index_dist(0, chars.size() - 1);
    for(int i = 0; i < 8; ++i) {
        std::cout << chars[index_dist(rng)];
    }
    std::cout << std::endl;
}


#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);
}


//Link to Boost
#define BOOST_TEST_DYN_LINK

//Define our Module name (prints at testing)
 #define BOOST_TEST_MODULE Conformal Bootstrap UnitTests

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cmath>
#include "test.h"
#include "../ConformalBlock.h"
#include "../BootstrapRunner.h"

BOOST_AUTO_TEST_SUITE(demo_suite)

//Name your test cases for what they test
BOOST_AUTO_TEST_CASE(demo1, * utf::label("demo"))
{
    int d = 4;
    int l = 6;
    int order = 7;
    double dlt12 = 0.6;
    double dlt34 = -0.3;
    double r = 0.124;
    double Pi = acos(-1.0);
    double eta = cos(Pi/3);
    double dlt = 8.3;
    ConformalBlockScalars cb(d, dlt12, dlt34);
    double res1 = cb.evaluate(dlt, l, r, eta, order);
    std::cout << "res1=" << res1 << std::endl;

    ConformalBlockScalars cb2(2, 0.01, 0.0);
    float_type actual = cb2.evaluate(0.0, 0, r, eta, 20);
    std::cout << "actual=" << actual << std::endl;

    std::cout << cb2.dDelta(0.0, 0, r, eta, 15) << std::endl;
    std::cout << cb2.dDelta12(0.0, 0, r, eta, 15) << std::endl;
    std::cout << cb2.dDelta34(0.0, 0, r, eta, 15) << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#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; }


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Linestring Example

#include <algorithm> // for reverse, unique
#include <iostream>
#include <iterator>
#include <utility>
#include <vector>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

// Optional includes and defines to handle c-arrays as points, std::vectors as linestrings
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::vector)
BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::deque)

template<typename P>
inline void translate_function(P& p)
{
        p.x(p.x() + 100.0);
}

template<typename P>
struct scale_functor
{
    inline void operator()(P& p)
    {
        p.x(p.x() * 1000.0);
        p.y(p.y() * 1000.0);
    }
};

template<typename Point>
struct round_coordinates
{
    typedef typename boost::geometry::coordinate_type<Point>::type coordinate_type;
    coordinate_type m_factor;

    inline round_coordinates(coordinate_type const& factor)
        : m_factor(factor)
    {}

    template <int Dimension>
    inline void round(Point& p)
    {
        coordinate_type c = boost::geometry::get<Dimension>(p) / m_factor;
        int rounded = c;
        boost::geometry::set<Dimension>(p, coordinate_type(rounded) * m_factor);
    }

    inline void operator()(Point& p)
    {
        round<0>(p);
        round<1>(p);
    }
};

int main(void)
{
    using namespace boost::geometry;

    // Define a linestring, which is a vector of points, and add some points
    // (we add them deliberately in different ways)
    typedef model::d2::point_xy<double> point_2d;
    typedef model::linestring<point_2d> linestring_2d;
    linestring_2d ls;

    // points can be created using "make" and added to a linestring using the std:: "push_back"
    ls.push_back(make<point_2d>(1.1, 1.1));

    // points can also be assigned using "assign_values" and added to a linestring using "append"
    point_2d lp;
    assign_values(lp, 2.5, 2.1);
    append(ls, lp);

    // Lines can be streamed using DSV (delimiter separated values)
    std::cout << dsv(ls) << std::endl;

    // The bounding box of linestrings can be calculated
    typedef model::box<point_2d> box_2d;
    box_2d b;
    envelope(ls, b);
    std::cout << dsv(b) << std::endl;

    // The length of the line can be calulated
    std::cout << "length: " << length(ls) << std::endl;

    // All things from std::vector can be called, because a linestring is a vector
    std::cout << "number of points 1: " << ls.size() << std::endl;

    // All things from boost ranges can be called because a linestring is considered as a range
    std::cout << "number of points 2: " << boost::size(ls) << std::endl;

    // Generic function from geometry/OGC delivers the same value
    std::cout << "number of points 3: " << num_points(ls) << std::endl;

    // The distance from a point to a linestring can be calculated
    point_2d p(1.9, 1.2);
    std::cout << "distance of " << dsv(p)
        << " to line: " << distance(p, ls) << std::endl;

    // A linestring is a vector. However, some algorithms consider "segments",
    // which are the line pieces between two points of a linestring.
    double d = distance(p, model::segment<point_2d >(ls.front(), ls.back()));
    std::cout << "distance: " << d << std::endl;

    // Add some three points more, let's do it using a classic array.
    // (See documentation for picture of this linestring)
    const double c[][2] = { {3.1, 3.1}, {4.9, 1.1}, {3.1, 1.9} };
    append(ls, c);
    std::cout << "appended: " << dsv(ls) << std::endl;

    // Output as iterator-pair on a vector
    {
        std::vector<point_2d> v;
        std::copy(ls.begin(), ls.end(), std::back_inserter(v));

        std::cout
            << "as vector: "
            << dsv(v)
            << std::endl;
    }

    // All algorithms from std can be used: a linestring is a vector
    std::reverse(ls.begin(), ls.end());
    std::cout << "reversed: " << dsv(ls) << std::endl;
    std::reverse(boost::begin(ls), boost::end(ls));

    // The other way, using a vector instead of a linestring, is also possible
    std::vector<point_2d> pv(ls.begin(), ls.end());
    std::cout << "length: " << length(pv) << std::endl;

    // If there are double points in the line, you can use unique to remove them
    // So we add the last point, print, make a unique copy and print
    {
        // (sidenote, we have to make copies, because
        // ls.push_back(ls.back()) often succeeds but
        // IS dangerous and erroneous!
        point_2d last = ls.back(), first = ls.front();
        ls.push_back(last);
        ls.insert(ls.begin(), first);
    }
    std::cout << "extra duplicate points: " << dsv(ls) << std::endl;

    {
        linestring_2d ls_copy;
        std::unique_copy(ls.begin(), ls.end(), std::back_inserter(ls_copy),
            boost::geometry::equal_to<point_2d>());
        ls = ls_copy;
        std::cout << "uniquecopy: " << dsv(ls) << std::endl;
    }

    // Lines can be simplified. This removes points, but preserves the shape
    linestring_2d ls_simplified;
    simplify(ls, ls_simplified, 0.5);
    std::cout << "simplified: " << dsv(ls_simplified) << std::endl;

    // for_each:
    // 1) Lines can be visited with std::for_each
    // 2) for_each_point is also defined for all geometries
    // 3) for_each_segment is defined for all geometries to all segments
    // 4) loop is defined for geometries to visit segments
    //    with state apart, and to be able to break out (not shown here)
    {
        linestring_2d lscopy = ls;
        std::for_each(lscopy.begin(), lscopy.end(), translate_function<point_2d>);
        for_each_point(lscopy, scale_functor<point_2d>());
        for_each_point(lscopy, translate_function<point_2d>);
        std::cout << "modified line: " << dsv(lscopy) << std::endl;
    }

    // Lines can be clipped using a clipping box. Clipped lines are added to the output iterator
    box_2d cb(point_2d(1.5, 1.5), point_2d(4.5, 2.5));

    std::vector<linestring_2d> clipped;
    intersection(cb, ls, clipped);

    // Also possible: clip-output to a vector of vectors
    std::vector<std::vector<point_2d> > vector_out;
    intersection(cb, ls, vector_out);

    std::cout << "clipped output as vector:" << std::endl;
    for (std::vector<std::vector<point_2d> >::const_iterator it
            = vector_out.begin(); it != vector_out.end(); ++it)
    {
        std::cout << dsv(*it) << std::endl;
    }

    // Calculate the convex hull of the linestring
    model::polygon<point_2d> hull;
    convex_hull(ls, hull);
    std::cout << "Convex hull:" << dsv(hull) << std::endl;

    // All the above assumed 2D Cartesian linestrings. 3D is possible as well
    // Let's define a 3D point ourselves, this time using 'float'
    typedef model::point<float, 3, cs::cartesian> point_3d;
    model::linestring<point_3d> line3;
    line3.push_back(make<point_3d>(1,2,3));
    line3.push_back(make<point_3d>(4,5,6));
    line3.push_back(make<point_3d>(7,8,9));

    // Not all algorithms work on 3d lines. For example convex hull does NOT.
    // But, for example, length, distance, simplify, envelope and stream do.
    std::cout << "3D: length: " << length(line3) << " line: " << dsv(line3) << std::endl;

    // With DSV you can also use other delimiters, e.g. JSON style
    std::cout << "JSON: "
        << dsv(ls, ", ", "[", "]", ", ", "[ ", " ]")
        << std::endl;

    return 0;
}


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


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/ulpdist.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], a2[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     a2[i] = (i%2) ? T(i+N) : T(-(i+N));
     b[i] = bs::ulpdist(a1[i], a2[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t bb(&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::ulpdist(aa1, aa2), bb);
}

STF_CASE_TPL("Check ulpdist on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" ulpdist real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulpdist;
  using p_t = bs::pack<T>;

  STF_EXPR_IS( ulpdist(p_t(), p_t()), p_t);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(ulpdist(bs::Inf<p_t>(), bs::Inf<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::Minf<p_t>(), bs::Minf<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::Nan<p_t>(), bs::Nan<p_t>()), bs::Zero<p_t>());
#endif

  STF_EQUAL(ulpdist(bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::One<p_t>(), bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());

  STF_EQUAL( ulpdist(bs::One<p_t>(), bs::One<p_t>()+bs::Eps<p_t>())
                , p_t(0.5)
                );

  STF_EQUAL( ulpdist(bs::One<p_t>(), bs::One<p_t>()-bs::Eps<p_t>())
                , p_t(0.5)
                );

  STF_EQUAL( ulpdist(bs::One<p_t>(), bs::One<p_t>()-bs::Eps<p_t>()/2)
                , p_t(0.25)
                );
}

STF_CASE_TPL (" ulpdist signed_integral",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulpdist;
  using p_t = bs::pack<T>;

  STF_EXPR_IS( ulpdist(p_t(), p_t()), p_t);

  STF_EQUAL(ulpdist(bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::One<p_t>(), bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());

  STF_EQUAL( ulpdist(bs::Zero<p_t>(), bs::Valmin<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmin<p_t>(), bs::Zero<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Zero<p_t>(), bs::Valmax<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmax<p_t>(), bs::Zero<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmin<p_t>(), bs::Valmax<p_t>())
                , bs::Valmax<p_t>()
                );
}

STF_CASE_TPL (" ulpdist unsigned_integral",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulpdist;
  using p_t = bs::pack<T>;

  STF_EXPR_IS( ulpdist(p_t(), p_t()), p_t);

  STF_EQUAL(ulpdist(bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::One<p_t>(), bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(ulpdist(bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());

  STF_EQUAL( ulpdist(bs::Zero<p_t>(), bs::Valmin<p_t>())
                , bs::Zero<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmin<p_t>(), bs::Zero<p_t>())
                , bs::Zero<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Zero<p_t>(), bs::Valmax<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmax<p_t>(), bs::Zero<p_t>())
                , bs::Valmax<p_t>()
                );

  STF_EQUAL( ulpdist(bs::Valmin<p_t>(), bs::Valmax<p_t>())
                , bs::Valmax<p_t>()
                );
}


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

#define BOOST_TEST_MAIN

#include <vector>
#include <limits>

#include <Eigen/Core>

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

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Mathematics/Statistics/kernelDensityDistribution.h"

namespace tudat
{
namespace unit_tests
{

//! Creates a std::vector of linear spaced values
std::vector< double > linspace( double start, double end, int N )
{
    std::vector < double > x( 0 );

    if ( N > 1 )
    {
        for( int i = 0; i < N; i++ )
        {
            x.push_back( start + ( end - start )*i /( N - 1.0 ) );
        }
    }
    return x;
}

//! Generator random vector using pseudo random generator
std::vector<Eigen::VectorXd> generateRandomVectorUniform(
        int seed, int numberOfSamples,
        Eigen::VectorXd lowerBound, Eigen::VectorXd upperBound)
{
    // Compute properties
    Eigen::VectorXd width = upperBound - lowerBound;
    Eigen::VectorXd average = (upperBound + lowerBound ) / 2.0;

    // Setup Random generator
    typedef boost::mt19937 RandomGeneratorType; // Mersenne Twister
    RandomGeneratorType randomGenerator(seed);              // Create random generator

    boost::uniform_real< > uniformDistribution( 0.0, 1.0 ); //
    boost::variate_generator< RandomGeneratorType, boost::uniform_real< > >
            Dice(randomGenerator, uniformDistribution); // define random generator

    std::vector< Eigen::VectorXd > randomSamples(numberOfSamples);
    Eigen::VectorXd randomSample( lowerBound.rows( ) );

    // Sample
    for(int i = 0; i < numberOfSamples; i++ ){ // Generate N samples
        for(int j = 0; j < randomSample.rows( ); j++){ // Generate vector of samples
            randomSample(j) = Dice( ) - 0.5;
        }
        randomSamples[i] = randomSample.cwiseProduct(width) + average;
    }
    return randomSamples;
}

BOOST_AUTO_TEST_SUITE( test_Kernel_Density_Distribution )

using tudat::mathematical_constants::PI;

//! Test whether optimal bandwidths are correctly computed (compared to Matlab results)
BOOST_AUTO_TEST_CASE( testOptimalKernelBandwidth )
{

    using namespace tudat::statistics;

    std::vector< Eigen::VectorXd > data( 0 );
    Eigen::VectorXd sample( 2 );
    sample << 2.2538, 1.177;
    data.push_back( sample );
    sample << 0.76529, 0.356;
    data.push_back( sample );
    sample << 1.5179, 1.14;
    data.push_back( sample );
    sample << 2.0972, 0.34093;
    data.push_back( sample );
    sample << 2.6727, 1.301;
    data.push_back( sample );
    sample << 2.8779, 0.48998;
    data.push_back( sample );
    sample << 1.6416, 0.27523;
    data.push_back( sample );
    sample << 0.41587, 0.35152;
    data.push_back( sample );
    sample << 0.44788, 0.86246;
    data.push_back( sample );
    sample << 0.77252, 0.6626;
    data.push_back( sample );

    KernelDensityDistribution distribution( data );

    // Generated expected Bandwidth using MATLAB
    Eigen::VectorXd expectedBandwidth( 2 );
    expectedBandwidth << 0.819018940571854, 0.263389630191125;

    Eigen::VectorXd computedOptimalBandwidth( 2 );
    computedOptimalBandwidth = distribution.getOptimalBandWidth( );

    // Compare Matlab against Tudal results; results are similar but not identical due to slightly different algorithms/
    BOOST_CHECK_CLOSE_FRACTION( expectedBandwidth( 0 ), computedOptimalBandwidth( 0 ), 2E-5 );
    BOOST_CHECK_CLOSE_FRACTION( expectedBandwidth( 1 ), computedOptimalBandwidth( 1 ), 2E-5 );

}

//! Test if the Epanechnikov distribution is correctly implemented.
BOOST_AUTO_TEST_CASE( testEpanechnikovKernel )
{
    tudat::statistics::EpanechnikovKernelDistribution distribution( 1.0, 2.0 );

    // Test theoretical values of Cdf
    BOOST_CHECK_CLOSE_FRACTION( distribution.evaluateCdf( 5.0 ), 1.0,
                                4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( distribution.evaluateCdf( 3.0 ), 1.0,
                                4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( distribution.evaluateCdf( 1.0 ), 0.5,
                                4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluateCdf( -1.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluateCdf( -2.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );

    // Test theoretical values of Pdf (zero outside of given range)
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( -1.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( 3.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( 4.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( -3.0 ) - 0.0 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );

    // Compare pdf and cdf at center points, compared to Matlab implementation
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( 0.0 ) - 0.28125 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluateCdf( 0.0 ) - 0.15625 ),
                       4.0 * std::numeric_limits< double >::epsilon( ) );
}

//! Test 1-Dimensional kernel density distribution, compared against Matlab implementation.
BOOST_AUTO_TEST_CASE( testKernelProbabilityDensity1D )
{
    using namespace tudat::statistics;

    Eigen::VectorXd sample( 1 );
    std::vector< Eigen::VectorXd > samples( 0 );

    sample( 0 ) = 3.0;
    samples.push_back( sample );
    sample( 0 ) = 1.0;
    samples.push_back( sample );
    sample( 0 ) = 4.0;
    samples.push_back( sample );
    sample( 0 ) = 7.0;
    samples.push_back( sample );

    // Create kernel
    KernelDensityDistribution distribution(samples, 1.0, KernelType::gaussian_kernel );

    Eigen::VectorXd location( 1 );
    // Case 1
    {
        // Compure pdf
        location << 5.396093156208099e-01;
        double computedDensity = distribution.evaluatePdf( location );

        // Data obtained from MATLAB ksdensity function
        double expectedDensity = 8.426926493705324e-02;

        // Check that the same bandwidths are used compared with MATLAB
        Eigen::VectorXd expectedBandwidth( 1 );
        expectedBandwidth << 1.785192502061299;

        Eigen::VectorXd computedBandwidth = distribution.getBandWidth( );
        Eigen::VectorXd difference = expectedBandwidth - computedBandwidth;

        BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        // Check pdf against Matlab computation
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Case 2
    {
        Eigen::VectorXd location( 1 );
        location << 3.915600227210264;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 1.320686749820565e-01;

        // Check pdf against Matlab computation
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Case 3
    {
        Eigen::VectorXd location( 1 );
        location << 8.135588866697081;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 5.036314749096298e-02;

        // Check pdf against Matlab computation
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

//! Test 2-Dimensional kernel density distribution, compared against Matlab implementation.
BOOST_AUTO_TEST_CASE( testKernelProbabilityDensity2D )
{
    using namespace tudat::statistics;

    Eigen::VectorXd sample( 2 );
    std::vector< Eigen::VectorXd > samples( 0 );

    sample << 3.0, 2E-2;
    samples.push_back( sample );
    sample << -1.0, 5E-2;
    samples.push_back( sample );
    sample << 5.0, 2E-1;
    samples.push_back( sample );
    sample << 2.0, 1E-3;
    samples.push_back( sample );

    // Create kernel
    KernelDensityDistribution distribution( samples, 1.0, KernelType::gaussian_kernel );

    // Case 1
    {
        Eigen::VectorXd location( 2 );
        location << 1.141869732328655, 2.995235933344523e-02;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 1.136916246361795;

        // Check that the same bandwidths are used compared with MATLAB
        Eigen::VectorXd expectedBandwidth( 2 );
        expectedBandwidth << 1.765086418052112, 2.882974482818450e-02;
        Eigen::VectorXd computedBandwidth = distribution.getBandWidth( );
        Eigen::VectorXd difference = expectedBandwidth - computedBandwidth;
        BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 4.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( std::fabs( difference( 1 ) ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Case 2
    {
        Eigen::VectorXd location( 2 );
        location << -2.862738183470956, 1.582207969089993e-01;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 4.038687551312721e-04;

        // Check pdf against Matlab computation
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Case 3
    {
        Eigen::VectorXd location( 2 );
        location << 6.862738183470957, 2.995235933344523e-02;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 7.784149895640656e-02;

        // Check pdf against Matlab computation
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Compare marginal CDF against Matlab
    {
        // Marginal CDF compare with MATLAB
        Eigen::VectorXd bandwidth( 2 );
        bandwidth << 1.785192502061299, 2.915814420033455e-02;
        distribution.setBandWidth( bandwidth );

         // Compare against Matlab results,
        BOOST_CHECK_CLOSE_FRACTION(
                    distribution.evaluateCumulativeMarginalProbability( 0, 2.276047714155371E-1 ),
                    2.446324562687310E-1, 5E-3 );
        BOOST_CHECK_CLOSE_FRACTION(
                    distribution.evaluateCumulativeMarginalProbability( 1, 6.083875672099921e-02 ),
                    6.360523509878839e-01, 5E-3 );
        BOOST_CHECK_CLOSE_FRACTION(
                    distribution.evaluateCumulativeMarginalProbability( 1, 9.861136936766661e-02 ),
                    7.371490745984073e-01, 5E-3 );

        // Check theoretical CDF at edge of domain.
        BOOST_CHECK_SMALL( std::fabs(
            distribution.evaluateCumulativeMarginalProbability( 1, 300.0 )
                               - 1.0 ), 4.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( std::fabs(
            distribution.evaluateCumulativeMarginalProbability( 1, -200.0 )
                               - 0.0 ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

//! Test whether scaling of sample standard deviation is done correctly
BOOST_AUTO_TEST_CASE( testStandardDeviationScaling )
{

    using namespace tudat::statistics;

    // Generate random datapoints
    Eigen::VectorXd mean( 2 );
    mean << 0.0, 0.0;
    Eigen::VectorXd standardDeviation( 2 );
    standardDeviation << 2.0, 0.5;
    Eigen::VectorXd lowerBound = mean - standardDeviation * std::sqrt( 3.0 );
    Eigen::VectorXd upperBound = mean + standardDeviation * std::sqrt( 3.0 );

    int numberOfSamples = 1E6;
    int seed = 100;
    std::vector< Eigen::VectorXd > samples = generateRandomVectorUniform(
                seed, numberOfSamples, lowerBound, upperBound );

    // Create distribution
    KernelDensityDistribution distribution2( samples );

    Eigen::VectorXd sampleMean = distribution2.getSampleMean( );
    Eigen::VectorXd sampleStandardDeviation = distribution2.getSampleStandardDeviation( );

    // Check sample mean and standard deviation
    BOOST_CHECK_SMALL( std::fabs( sampleMean( 0 ) - 0.0 ), 5E-3 );
    BOOST_CHECK_SMALL( std::fabs( sampleMean( 1 ) - 0.0 ), 5E-3 );
    BOOST_CHECK_SMALL( std::fabs( sampleStandardDeviation( 0 ) - 2.0 ), 1E-3 );
    BOOST_CHECK_SMALL( std::fabs( sampleStandardDeviation( 1 ) - 0.5 ), 1E-3 );

    // Check whether probability density function is approximately uniform.
    double probabilityDensity = 1.0 / ( ( upperBound( 0 ) - lowerBound( 0 ) ) * ( upperBound( 1 ) - lowerBound( 1 ) ) );

    Eigen::VectorXd x( 2 );
    x << 0.0, 0.0;
    BOOST_CHECK_SMALL( std::fabs( distribution2.evaluatePdf( x ) - probabilityDensity ), 2E-3 );

    x << -1.0, 0.3;
    BOOST_CHECK_CLOSE_FRACTION( distribution2.evaluatePdf( x ), probabilityDensity, 2E-4 );

    // Set standard deviation scaling
    Eigen::VectorXd standardDeviationAdjusted( 2 );
    standardDeviationAdjusted << 0.5, 3.0;

    // Create scaled kernel distribution
    KernelDensityDistribution distribution( samples, 1.0, KernelType::gaussian_kernel, standardDeviationAdjusted );

    Eigen::VectorXd newSampleMean = distribution.getSampleMean( );
    Eigen::VectorXd newSampleStandardDeviation = distribution.getSampleStandardDeviation( );

    // Recheck mean.
    BOOST_CHECK_SMALL( std::fabs( newSampleMean( 0 ) - 0.0 ), 5E-3 );
    BOOST_CHECK_SMALL( std::fabs( newSampleMean( 1 ) - 0.0 ), 5E-3 );

    // Check whether sample standard deviation conforms to required values.
    BOOST_CHECK_SMALL( std::fabs( newSampleStandardDeviation( 0 ) - 0.5 ), 1E-13 );
    BOOST_CHECK_SMALL( std::fabs( newSampleStandardDeviation( 1 ) - 3.0 ), 1E-13 );

    // Check whether probability density function is approximately uniform.
    lowerBound = mean - standardDeviationAdjusted * std::sqrt( 3.0 );
    upperBound = mean + standardDeviationAdjusted * std::sqrt( 3.0 );
    probabilityDensity = 1.0 / ( ( upperBound( 0 ) - lowerBound( 0 ) ) * ( upperBound( 1 ) - lowerBound( 1 ) ) );

    x << 0.0, 0.0;
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( x ) - probabilityDensity ), 1.0E-3 );

    x << -0.2, 2.3;
    BOOST_CHECK_CLOSE_FRACTION( distribution.evaluatePdf( x ), probabilityDensity, 1.0E-3 );
}

//! Test 3-Dimensional kernel density distribution, compared against Matlab implementation.
BOOST_AUTO_TEST_CASE( testKernelProbabilityDensity3D )
{
    using namespace tudat::statistics;

    Eigen::VectorXd sample( 3 );
    std::vector< Eigen::VectorXd > samples( 0 );

    sample << 3.0, 2E-2, 0.1;
    samples.push_back( sample );
    sample << -1.0, 5E-2, 1.0;
    samples.push_back( sample );
    sample << 5.0, 2E-1, 3.0;
    samples.push_back( sample );
    sample << 2.0, 1E-3, 1.5;
    samples.push_back( sample );

    KernelDensityDistribution distribution(samples, 1.0, KernelType::gaussian_kernel );

    // Test probability density
    {
        Eigen::VectorXd location( 3 );
        location <<  1.5, 1E-2, 0.8;

        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = 4.653809309094347e-01;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test marginal cumulative probability
    {
        Eigen::VectorXd location( 3 );
        location <<  1.5, 1E-2, 0.8;

        int marginal = 0;
        double computedDensity = distribution.evaluateCumulativeMarginalProbability( marginal, location( marginal ) );
        double expectedDensity = 3.829580307170373e-01;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        marginal = 1;
        computedDensity = distribution.evaluateCumulativeMarginalProbability( marginal, location( marginal ) );
        expectedDensity = 2.674493565829487e-01;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test conditional marginal cumulative probability
    {
        Eigen::VectorXd location( 3 );
        location <<  1.5, 1E-2, 0.8;

        int marginal = 0;
        std::vector< int > conditionDimensions( 0 );
        conditionDimensions.push_back( 1 );
        std::vector< double > conditions( 0 );
        conditions.push_back( 0.1 );

        double computedDensity = distribution.evaluateCumulativeConditionalMarginalProbability(
                    conditionDimensions, conditions, marginal, location( marginal ) );
        double expectedDensity = 4.970339167925200e-01;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        conditionDimensions[0] = 0;
        conditions[0] = 0.4;
        marginal = 2;
        computedDensity = distribution.evaluateCumulativeConditionalMarginalProbability(
                    conditionDimensions, conditions, marginal, location( marginal ) );
        expectedDensity = 3.932443313127392e-01;

        // Check correct density
        BOOST_CHECK_CLOSE_FRACTION( computedDensity, expectedDensity, 4.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

//! Test kernel density function for uncorrelated input data.
BOOST_AUTO_TEST_CASE( testProbabilityFunctionsUncorrelated2D )
{
    using namespace tudat::statistics;

    Eigen::VectorXd sample( 2 );
    std::vector< Eigen::VectorXd > samples( 0 );

    // Define uncorrelated input data.
    sample << 3.0, 0.1 ;
    samples.push_back( sample );
    sample << 5.0, 0.1;
    samples.push_back( sample );
    sample << 7.0, 0.1;
    samples.push_back( sample );

    // Manually define bandwidth
    Eigen::VectorXd bandWidth( 2 );
    bandWidth << 1.0, 0.3;

    KernelDensityDistribution distribution( samples, 1.0, KernelType::gaussian_kernel, Eigen::VectorXd::Zero( 0 ), bandWidth );

    // Create manual Gaussian distributions
    std::shared_ptr< ContinuousProbabilityDistribution< double > > gaussianDistribution1 =
            createBoostRandomVariable( normal_boost_distribution, { samples[ 0 ]( 0 ), 1.0  } );

    std::shared_ptr< ContinuousProbabilityDistribution< double > > gaussianDistribution2 =
            createBoostRandomVariable( normal_boost_distribution, { samples[ 1 ]( 0 ), 1.0  } );

    std::shared_ptr< ContinuousProbabilityDistribution< double > > gaussianDistribution3 =
            createBoostRandomVariable( normal_boost_distribution, { samples[ 2 ]( 0 ), 1.0  } );

    std::shared_ptr< ContinuousProbabilityDistribution< double > > gaussianDistribution4 =
            createBoostRandomVariable( normal_boost_distribution, { samples[ 0 ]( 1 ), 0.3  } );

    // Test probability density
    {
        Eigen::VectorXd location( 2 );
        location <<  1.5, 1E-2;

        // Compute pdf from kernel and theoretical value.
        double computedDensity = distribution.evaluatePdf( location );
        double expectedDensity = ( gaussianDistribution1->evaluatePdf( location( 0 ) ) +
                                   gaussianDistribution2->evaluatePdf( location( 0 ) ) +
                                   gaussianDistribution3->evaluatePdf( location( 0 ) ) ) *
                gaussianDistribution4->evaluatePdf( location( 1 ) ) / 3.0;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test cumulative probability
    {
        Eigen::VectorXd location( 2 );
        location <<  2.5, 1E-1;

        // Compute cdf from kernel and theoretical value.
        double computedDensity = distribution.evaluateCdf( location );
        double expectedDensity = (gaussianDistribution1->evaluateCdf( location( 0 ) ) +
                                  gaussianDistribution2->evaluateCdf( location( 0 ) ) +
                                  gaussianDistribution3->evaluateCdf( location( 0 ) ) ) *
                gaussianDistribution4->evaluateCdf( location( 1 ) ) / 3.0;

        // Check correct density
        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test marginal cumulative probability
    {
        Eigen::VectorXd location( 2 );
        location <<  2.5, 1E-1;
        int marginal = 0;

        // Compute marginal cdf from kernel and theoretical value (marginalDimension = 0).
        double computedDensity = distribution.evaluateCumulativeMarginalProbability( marginal, location( marginal ) );
        double expectedDensity = ( gaussianDistribution1->evaluateCdf( location( 0 ) ) +
                                   gaussianDistribution2->evaluateCdf( location( 0 ) ) +
                                   gaussianDistribution3->evaluateCdf( location( 0 ) ) ) / 3.0;

        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        // Compute marginal cdf from kernel and theoretical value (marginalDimension = 1).
        marginal = 1;
        computedDensity = distribution.evaluateCumulativeMarginalProbability( marginal, location( marginal ) );
        expectedDensity = gaussianDistribution4->evaluateCdf( location( 1 ) );

        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }

    // Test conditional marginal cumulative probability
    {
        double location;
        std::vector< int > conditionDimensions( 0 );
        std::vector< double > conditions( 0 );

        location = 2.5;
        int marginal = 0;
        conditionDimensions.push_back( 1 );
        conditions.push_back( 0.2 );

        // Compute marginal cdf from kernel and theoretical value (marginalDimension = 0).
        double computedDensity = distribution.evaluateCumulativeConditionalMarginalProbability(
                    conditionDimensions, conditions, marginal, location );
        double expectedDensity = ( gaussianDistribution1->evaluateCdf( location ) +
                                   gaussianDistribution2->evaluateCdf( location ) +
                                   gaussianDistribution3->evaluateCdf( location ) ) / 3.0;

        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );

        // Compute conditional marginal cdf from kernel and theoretical value (marginalDimension = 1).
        marginal = 1;
        conditionDimensions[0] = 0;
        conditions[ 0 ]  = 2.5;
        location = 0.2;
        computedDensity = distribution.evaluateCumulativeConditionalMarginalProbability(
                    conditionDimensions, conditions, marginal, location );
        expectedDensity = gaussianDistribution4->evaluateCdf( location );

        BOOST_CHECK_SMALL( std::fabs( computedDensity - expectedDensity ), 4.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2f M = Matrix2f::Random();
Matrix2f m;
m = M;
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Now we want to copy a column into a row." << endl;
cout << "If we do m.col(1) = m.row(0), then m becomes:" << endl;
m.col(1) = m.row(0);
cout << m << endl << "which is wrong!" << endl;
cout << "Now let us instead do m.col(1) = m.row(0).eval(). Then m becomes" << endl;
m = M;
m.col(1) = m.row(0).eval();
cout << m << endl << "which is right." << endl;

  return 0;
}


#ifndef plane_calibration_SRC_PLANE_TO_DEPTH_IMAGE_HPP_
#define plane_calibration_SRC_PLANE_TO_DEPTH_IMAGE_HPP_

#include <Eigen/Dense>
#include <utility>
#include <sstream>
#include <memory>

#include "camera_model.hpp"

namespace plane_calibration
{

class PlaneToDepthImage
{
public:
  class Errors
  {
  public:
    std::string asPrintString()
    {
      std::ostringstream string_stream;
      string_stream << mean << ", " << min << ", " << max;
      return string_stream.str();
    }

    double mean;
    double min;
    double max;
  };

  PlaneToDepthImage(const CameraModel::Parameters& camera_model_paramaters);
  Eigen::MatrixXf convert(const Eigen::Affine3d& plane_transformation);

  static Eigen::MatrixXf convert(const Eigen::Affine3d& plane_transformation,
                                 const CameraModel::Parameters& camera_model_paramaters);
  static Eigen::MatrixXf convert(const Eigen::Affine3d& plane_transformation,
                                 const CameraModel::Parameters& camera_model_paramaters,
                                 const std::pair<Eigen::MatrixXd, Eigen::MatrixXd>& xy_multipliers);

  static std::pair<Eigen::MatrixXd, Eigen::MatrixXd> depthCalculationXYMultiplier(
      const CameraModel::Parameters& camera_model_paramaters);

  static Errors getErrors(const Eigen::Affine3d& plane_transformation,
                          const CameraModel::Parameters& camera_model_paramaters, Eigen::MatrixXf image_matrix);

protected:
  CameraModel::Parameters camera_model_paramaters_;
  std::pair<Eigen::MatrixXd, Eigen::MatrixXd> xy_multipliers_;
};
typedef std::shared_ptr<PlaneToDepthImage> PlaneToDepthImagePtr;

} /* end namespace */

#endif


#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