//  (C) Copyright Nick Thompson 2018.
//  (C) Copyright Matt Borland 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP
#define BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP

#include <boost/math/statistics/detail/single_pass.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/assert.hpp>
#include <algorithm>
#include <iterator>
#include <tuple>
#include <cmath>
#include <vector>
#include <type_traits>
#include <utility>
#include <numeric>
#include <list>

// Support compilers with P0024R2 implemented without linking TBB
// https://en.cppreference.com/w/cpp/compiler_support
#ifndef BOOST_NO_CXX17_HDR_EXECUTION
#include <execution>

namespace boost::math::statistics {

template<class ExecutionPolicy, class ForwardIterator>
inline auto mean(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return detail::mean_sequential_impl<double>(first, last);
        }
        else
        {
            return std::reduce(exec, first, last, 0.0) / std::distance(first, last);
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return detail::mean_sequential_impl<Real>(first, last);
        }
        else
        {
            return std::reduce(exec, first, last, Real(0.0)) / Real(std::distance(first, last));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto mean(ExecutionPolicy&& exec, Container const & v)
{
    return mean(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto mean(ForwardIterator first, ForwardIterator last)
{
    return mean(std::execution::seq, first, last);
}

template<class Container>
inline auto mean(Container const & v)
{
    return mean(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
           return std::get<2>(detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::get<1>(results) / std::get<4>(results);
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return std::get<2>(detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::get<1>(results) / std::get<4>(results);
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto variance(ExecutionPolicy&& exec, Container const & v)
{
    return variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto variance(ForwardIterator first, ForwardIterator last)
{
    return variance(std::execution::seq, first, last);
}

template<class Container>
inline auto variance(Container const & v)
{
    return variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto sample_variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(exec, first, last)/(n-1);
}

template<class ExecutionPolicy, class Container>
inline auto sample_variance(ExecutionPolicy&& exec, Container const & v)
{
    return sample_variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto sample_variance(ForwardIterator first, ForwardIterator last)
{
    return sample_variance(std::execution::seq, first, last);
}

template<class Container>
inline auto sample_variance(Container const & v)
{
    return sample_variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto mean_and_sample_variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<2>(results)*std::get<3>(results)/(std::get<3>(results)-1.0));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<1>(results) / (std::get<4>(results)-1.0));
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<2>(results)*std::get<3>(results)/(std::get<3>(results)-Real(1)));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<1>(results) / (std::get<4>(results)-Real(1)));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto mean_and_sample_variance(ExecutionPolicy&& exec, Container const & v)
{
    return mean_and_sample_variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto mean_and_sample_variance(ForwardIterator first, ForwardIterator last)
{
    return mean_and_sample_variance(std::execution::seq, first, last);
}

template<class Container>
inline auto mean_and_sample_variance(Container const & v)
{
    return mean_and_sample_variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto first_four_moments(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::first_four_moments_sequential_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                std::get<3>(results) / std::get<4>(results));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::first_four_moments_sequential_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto first_four_moments(ExecutionPolicy&& exec, Container const & v)
{
    return first_four_moments(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto first_four_moments(ForwardIterator first, ForwardIterator last)
{
    return first_four_moments(std::execution::seq, first, last);
}

template<class Container>
inline auto first_four_moments(Container const & v)
{
    return first_four_moments(std::execution::seq, std::cbegin(v), std::cend(v));
}

// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ExecutionPolicy, class ForwardIterator>
inline auto skewness(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    using std::sqrt;

    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            return detail::skewness_sequential_impl<double>(first, last);
        }
        else
        {
            return detail::skewness_sequential_impl<Real>(first, last);
        }
    }
    else
    {
        const auto [M1, M2, M3, M4] = first_four_moments(exec, first, last);
        const auto n = std::distance(first, last);
        const auto var = M2/(n-1);

        if (M2 == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            if constexpr (std::is_integral_v<Real>)
            {
                return double(0);
            }
            else
            {
                return Real(0);
            }
        }
        else
        {
            return M3/(M2*sqrt(var)) / Real(2);
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto skewness(ExecutionPolicy&& exec, Container & v)
{
    return skewness(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto skewness(ForwardIterator first, ForwardIterator last)
{
    return skewness(std::execution::seq, first, last);
}

template<class Container>
inline auto skewness(Container const & v)
{
    return skewness(std::execution::seq, std::cbegin(v), std::cend(v));
}

// Follows equation 1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ExecutionPolicy, class ForwardIterator>
inline auto kurtosis(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    const auto [M1, M2, M3, M4] = first_four_moments(exec, first, last);
    if (M2 == 0)
    {
        return M2;
    }
    return M4/(M2*M2);
}

template<class ExecutionPolicy, class Container>
inline auto kurtosis(ExecutionPolicy&& exec, Container const & v)
{
    return kurtosis(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto kurtosis(ForwardIterator first, ForwardIterator last)
{
    return kurtosis(std::execution::seq, first, last);
}

template<class Container>
inline auto kurtosis(Container const & v)
{
    return kurtosis(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto excess_kurtosis(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    return kurtosis(exec, first, last) - 3;
}

template<class ExecutionPolicy, class Container>
inline auto excess_kurtosis(ExecutionPolicy&& exec, Container const & v)
{
    return excess_kurtosis(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto excess_kurtosis(ForwardIterator first, ForwardIterator last)
{
    return excess_kurtosis(std::execution::seq, first, last);
}

template<class Container>
inline auto excess_kurtosis(Container const & v)
{
    return excess_kurtosis(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
auto median(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(exec, first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(exec, first, middle, last);
        std::nth_element(exec, middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto median(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return median(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto median(RandomAccessIterator first, RandomAccessIterator last)
{
    return median(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto median(RandomAccessContainer & v)
{
    return median(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
inline auto gini_coefficient(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;

    if(!std::is_sorted(exec, first, last))
    {
        std::sort(exec, first, last);
    }

    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            return detail::gini_coefficient_sequential_impl<double>(first, last);
        }
        else
        {
            return detail::gini_coefficient_sequential_impl<Real>(first, last);
        }
    }

    else if constexpr (std::is_integral_v<Real>)
    {
        return detail::gini_coefficient_parallel_impl<double>(exec, first, last);
    }

    else
    {
        return detail::gini_coefficient_parallel_impl<Real>(exec, first, last);
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto gini_coefficient(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return gini_coefficient(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    return gini_coefficient(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto gini_coefficient(RandomAccessContainer & v)
{
    return gini_coefficient(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
inline auto sample_gini_coefficient(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(exec, first, last)/(n-1);
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto sample_gini_coefficient(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return sample_gini_coefficient(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    return sample_gini_coefficient(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto sample_gini_coefficient(RandomAccessContainer & v)
{
    return sample_gini_coefficient(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
auto median_absolute_deviation(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last,
    typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::statistics::median(exec, first, last);
    }
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(exec, first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(exec, first, middle, last, comparator);
        std::nth_element(exec, middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto median_absolute_deviation(ExecutionPolicy&& exec, RandomAccessContainer & v,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(exec, std::begin(v), std::end(v), center);
}

template<class RandomAccessIterator>
inline auto median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last,
    typename RandomAccessIterator::value_type center=std::numeric_limits<typename RandomAccessIterator::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::execution::seq, first, last, center);
}

template<class RandomAccessContainer>
inline auto median_absolute_deviation(RandomAccessContainer & v,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::execution::seq, std::begin(v), std::end(v), center);
}

template<class ExecutionPolicy, class ForwardIterator>
auto interquartile_range(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    static_assert(!std::is_integral_v<Real>, "Integer values have not yet been implemented.");
    auto m = std::distance(first,last);
    BOOST_MATH_ASSERT_MSG(m >= 3, "At least 3 samples are required to compute the interquartile range.");
    auto k = m/4;
    auto j = m - (4*k);
    // m = 4k+j.
    // If j = 0 or j = 1, then there are an even number of samples below the median, and an even number above the median.
    //    Then we must average adjacent elements to get the quartiles.
    // If j = 2 or j = 3, there are an odd number of samples above and below the median, these elements may be directly extracted to get the quartiles.

    if (j==2 || j==3)
    {
        auto q1 = first + k;
        auto q3 = first + 3*k + j - 1;
        std::nth_element(exec, first, q1, last);
        Real Q1 = *q1;
        std::nth_element(exec, q1, q3, last);
        Real Q3 = *q3;
        return Q3 - Q1;
    } else {
        // j == 0 or j==1:
        auto q1 = first + k - 1;
        auto q3 = first + 3*k - 1 + j;
        std::nth_element(exec, first, q1, last);
        Real a = *q1;
        std::nth_element(exec, q1, q1 + 1, last);
        Real b = *(q1 + 1);
        Real Q1 = (a+b)/2;
        std::nth_element(exec, q1, q3, last);
        a = *q3;
        std::nth_element(exec, q3, q3 + 1, last);
        b = *(q3 + 1);
        Real Q3 = (a+b)/2;
        return Q3 - Q1;
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto interquartile_range(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return interquartile_range(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto interquartile_range(RandomAccessIterator first, RandomAccessIterator last)
{
    return interquartile_range(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto interquartile_range(RandomAccessContainer & v)
{
    return interquartile_range(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class ForwardIterator, class OutputIterator>
inline OutputIterator mode(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(exec, first, last))
    {
        if constexpr (std::is_same_v<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>)
        {
            std::sort(exec, first, last);
        }
        else
        {
            BOOST_MATH_ASSERT("Data must be sorted for sequential mode calculation");
        }
    }

    return detail::mode_impl(first, last, output);
}

template<class ExecutionPolicy, class Container, class OutputIterator>
inline OutputIterator mode(ExecutionPolicy&& exec, Container & v, OutputIterator output)
{
    return mode(exec, std::begin(v), std::end(v), output);
}

template<class ForwardIterator, class OutputIterator>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    return mode(std::execution::seq, first, last, output);
}

// Requires enable_if_t to not clash with impl that returns std::list
// Very ugly. std::is_execution_policy_v returns false for the std::execution objects and decltype of the objects (e.g. std::execution::seq)
template<class Container, class OutputIterator, std::enable_if_t<!std::is_convertible_v<std::execution::sequenced_policy, Container> &&
                                                                 !std::is_convertible_v<std::execution::parallel_unsequenced_policy, Container> &&
                                                                 !std::is_convertible_v<std::execution::parallel_policy, Container>
                                                                 #if __cpp_lib_execution > 201900
                                                                 && !std::is_convertible_v<std::execution::unsequenced_policy, Container>
                                                                 #endif
                                                                 , bool> = true>
inline OutputIterator mode(Container & v, OutputIterator output)
{
    return mode(std::execution::seq, std::begin(v), std::end(v), output);
}

// std::list is the return type for the proposed STL stats library

template<class ExecutionPolicy, class ForwardIterator, class Real = typename std::iterator_traits<ForwardIterator>::value_type>
inline auto mode(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    std::list<Real> modes;
    mode(exec, first, last, std::inserter(modes, modes.begin()));
    return modes;
}

template<class ExecutionPolicy, class Container>
inline auto mode(ExecutionPolicy&& exec, Container & v)
{
    return mode(exec, std::begin(v), std::end(v));
}

template<class ForwardIterator>
inline auto mode(ForwardIterator first, ForwardIterator last)
{
    return mode(std::execution::seq, first, last);
}

template<class Container>
inline auto mode(Container & v)
{
    return mode(std::execution::seq, std::begin(v), std::end(v));
}

} // Namespace boost::math::statistics

#else // Backwards compatible bindings for C++11

namespace boost { namespace math { namespace statistics {

template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double mean(const ForwardIterator first, const ForwardIterator last)
{
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    return detail::mean_sequential_impl<double>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double mean(const Container& c)
{
    return mean(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real mean(const ForwardIterator first, const ForwardIterator last)
{
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    return detail::mean_sequential_impl<Real>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real mean(const Container& c)
{
    return mean(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double variance(const ForwardIterator first, const ForwardIterator last)
{
    return std::get<2>(detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double variance(const Container& c)
{
    return variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real variance(const ForwardIterator first, const ForwardIterator last)
{
    return std::get<2>(detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last));

}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real variance(const Container& c)
{
    return variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_variance(const Container& c)
{
    return sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_variance(const Container& c)
{
    return sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::pair<double, double> mean_and_sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last);
    return std::make_pair(std::get<0>(results), std::get<3>(results)*std::get<2>(results)/(std::get<3>(results)-1.0));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::pair<double, double> mean_and_sample_variance(const Container& c)
{
    return mean_and_sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::pair<Real, Real> mean_and_sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last);
    return std::make_pair(std::get<0>(results), std::get<3>(results)*std::get<2>(results)/(std::get<3>(results)-Real(1)));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::pair<Real, Real> mean_and_sample_variance(const Container& c)
{
    return mean_and_sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::tuple<double, double, double, double> first_four_moments(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::first_four_moments_sequential_impl<std::tuple<double, double, double, double, double>>(first, last);
    return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                           std::get<3>(results) / std::get<4>(results));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::tuple<double, double, double, double> first_four_moments(const Container& c)
{
    return first_four_moments(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::tuple<Real, Real, Real, Real> first_four_moments(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::first_four_moments_sequential_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
    return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                           std::get<3>(results) / std::get<4>(results));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::tuple<Real, Real, Real, Real> first_four_moments(const Container& c)
{
    return first_four_moments(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double skewness(const ForwardIterator first, const ForwardIterator last)
{
    return detail::skewness_sequential_impl<double>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double skewness(const Container& c)
{
    return skewness(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real skewness(const ForwardIterator first, const ForwardIterator last)
{
    return detail::skewness_sequential_impl<Real>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real skewness(const Container& c)
{
    return skewness(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    std::tuple<double, double, double, double> M = first_four_moments(first, last);

    if(std::get<1>(M) == 0)
    {
        return std::get<1>(M);
    }
    else
    {
        return std::get<3>(M)/(std::get<1>(M)*std::get<1>(M));
    }
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double kurtosis(const Container& c)
{
    return kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    std::tuple<Real, Real, Real, Real> M = first_four_moments(first, last);

    if(std::get<1>(M) == 0)
    {
        return std::get<1>(M);
    }
    else
    {
        return std::get<3>(M)/(std::get<1>(M)*std::get<1>(M));
    }
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real kurtosis(const Container& c)
{
    return kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double excess_kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double excess_kurtosis(const Container& c)
{
    return excess_kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real excess_kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real excess_kurtosis(const Container& c)
{
    return excess_kurtosis(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type>
Real median(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last);
        std::nth_element(middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type>
inline Real median(RandomAccessContainer& c)
{
    return median(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::gini_coefficient_sequential_impl<double>(first, last);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double gini_coefficient(RandomAccessContainer& c)
{
    return gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::gini_coefficient_sequential_impl<Real>(first, last);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real gini_coefficient(RandomAccessContainer& c)
{
    return gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_gini_coefficient(RandomAccessContainer& c)
{
    return sample_gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_gini_coefficient(RandomAccessContainer& c)
{
    return sample_gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type>
Real median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last,
    typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::statistics::median(first, last);
    }
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last, comparator);
        std::nth_element(middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type>
inline Real median_absolute_deviation(RandomAccessContainer& c,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::begin(c), std::end(c), center);
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type>
Real interquartile_range(ForwardIterator first, ForwardIterator last)
{
    static_assert(!std::is_integral<Real>::value, "Integer values have not yet been implemented.");
    auto m = std::distance(first,last);
    BOOST_MATH_ASSERT_MSG(m >= 3, "At least 3 samples are required to compute the interquartile range.");
    auto k = m/4;
    auto j = m - (4*k);
    // m = 4k+j.
    // If j = 0 or j = 1, then there are an even number of samples below the median, and an even number above the median.
    //    Then we must average adjacent elements to get the quartiles.
    // If j = 2 or j = 3, there are an odd number of samples above and below the median, these elements may be directly extracted to get the quartiles.

    if (j==2 || j==3)
    {
        auto q1 = first + k;
        auto q3 = first + 3*k + j - 1;
        std::nth_element(first, q1, last);
        Real Q1 = *q1;
        std::nth_element(q1, q3, last);
        Real Q3 = *q3;
        return Q3 - Q1;
    }
    else
    {
        // j == 0 or j==1:
        auto q1 = first + k - 1;
        auto q3 = first + 3*k - 1 + j;
        std::nth_element(first, q1, last);
        Real a = *q1;
        std::nth_element(q1, q1 + 1, last);
        Real b = *(q1 + 1);
        Real Q1 = (a+b)/2;
        std::nth_element(q1, q3, last);
        a = *q3;
        std::nth_element(q3, q3 + 1, last);
        b = *(q3 + 1);
        Real Q3 = (a+b)/2;
        return Q3 - Q1;
    }
}

template<class Container, typename Real = typename Container::value_type>
Real interquartile_range(Container& c)
{
    return interquartile_range(std::begin(c), std::end(c));
}

template<class ForwardIterator, class OutputIterator,
    enable_if_t<std::is_same<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>::value, bool> = true>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::mode_impl(first, last, output);
}

template<class ForwardIterator, class OutputIterator,
    enable_if_t<!std::is_same<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>::value, bool> = true>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(first, last))
    {
        BOOST_MATH_ASSERT("Data must be sorted for mode calculation");
    }

    return detail::mode_impl(first, last, output);
}

template<class Container, class OutputIterator>
inline OutputIterator mode(Container& c, OutputIterator output)
{
    return mode(std::begin(c), std::end(c), output);
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type>
inline std::list<Real> mode(ForwardIterator first, ForwardIterator last)
{
    std::list<Real> modes;
    mode(first, last, std::inserter(modes, modes.begin()));
    return modes;
}

template<class Container, typename Real = typename Container::value_type>
inline std::list<Real> mode(Container& c)
{
    return mode(std::begin(c), std::end(c));
}
}}}
#endif
#endif // BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP


#include "activation.hpp"
#include "layer.hpp"

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

void init_layer(py::module& main)
{
    auto m  = main.def_submodule("layers");
    m.doc() = R"pbdoc(
        Network Layers
        -----------------------
        .. currentmodule:: layer
        .. autosummary::
           :toctree: _generate
           DenseLayer
    )pbdoc";

    py::class_<layer::DenseLayer>(m, "DenseLayer")
        .def(py::init<int, int, const activation::ActivationFunction&>(),
             py::arg("inputs"),
             py::arg("outputs"),
             py::arg("activation") = activation::identity)
        .def(py::init<int, int, const activation::ReluActivation&>(),
             py::arg("inputs"),
             py::arg("outputs"),
             py::arg("activation"))
        .def(py::init<int, int, const activation::TanhActivation&>(),
             py::arg("inputs"),
             py::arg("outputs"),
             py::arg("activation"))
        .def(py::init<int, int, const activation::ActivationFunction&, Real>(),
             py::arg("inputs"),
             py::arg("outputs"),
             py::arg("activation"),
             py::arg("scale_factor"))
        .def("evaluate", &layer::DenseLayer::forward)
        .def_property_readonly(
            "weights", py::overload_cast<>(&layer::DenseLayer::getWeights, py::const_))
        .def_property_readonly(
            "biases", py::overload_cast<>(&layer::DenseLayer::getBiases, py::const_))
        .def_property_readonly("weights_gradient",
                               &layer::DenseLayer::getWeightsGradient)
        .def_property_readonly("bias_gradient", &layer::DenseLayer::getBiasGradient);
}


#include "ros/ros.h"

#include "real_time_simulator/FSM.h"
#include "real_time_simulator/State.h"
#include "real_time_simulator/Sensor.h"

#include "real_time_simulator/Control.h"

#include "geometry_msgs/Vector3.h"
#include "std_msgs/String.h"

#include <time.h>
#include <sstream>
#include <string>
#include <iostream>
#include <fstream>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

#include <chrono>
#include <random>

using namespace Eigen;

class Rocket {
public:
    float dry_mass;
    float propellant_mass;
    float Isp;
    float minTorque;
    float maxTorque;

    std::vector<float> maxThrust{0, 0, 0};
    std::vector<float> minThrust{0, 0, 0};

    float dry_CM;
    float propellant_CM;
    float total_CM; // Current Cm of rocket, in real time

    float total_length;

    float initial_speed;

    float h0;

    std::vector<float> target_apogee = {0, 0, 0};
    std::vector<float> Cd = {0, 0, 0};
    std::vector<float> surface = {0, 0, 0};
    std::vector<float> drag_coeff = {0, 0, 0};

    std::vector<float> dry_Inertia{0, 0, 0};
    std::vector<float> total_Inertia{0, 0, 0};

    std::vector<float> J_inv{0, 0, 0};

    // Sensor data
    Vector3d sensor_acc;
    Vector3d sensor_gyro;
    float sensor_baro;

    float acc_noise, acc_bias;
    float gyro_noise, gyro_bias;
    float baro_noise, baro_bias;

    using state = Matrix<double, 14, 1>;
    using control = Matrix<double, 3, 2>;

    void update_CM(float current_prop_mass) {
        total_CM = total_length - (dry_CM * dry_mass + propellant_CM * current_prop_mass) /
                                  (dry_mass + current_prop_mass); // From aft of rocket

        float new_inertia = dry_Inertia[0] + pow(total_CM - (total_length - propellant_CM), 2) * current_prop_mass;

        total_Inertia[0] = new_inertia;
        total_Inertia[1] = new_inertia;

        J_inv[0] = total_CM / total_Inertia[0];
        J_inv[1] = total_CM / total_Inertia[1];
        J_inv[2] = 1 / total_Inertia[2];
    }

    void init(ros::NodeHandle n) {
        n.getParam("/rocket/minTorque", minTorque);
        n.getParam("/rocket/maxTorque", maxTorque);
        n.getParam("/rocket/maxThrust", maxThrust);
        n.getParam("/rocket/minThrust", minThrust);
        n.getParam("/rocket/Isp", Isp);

        n.getParam("/rocket/dry_mass", dry_mass);
        n.getParam("/rocket/propellant_mass", propellant_mass);

        n.getParam("/rocket/Cd", Cd);
        n.getParam("/rocket/dry_I", dry_Inertia);

        n.getParam("/rocket/dry_CM", dry_CM);
        n.getParam("/rocket/propellant_CM", propellant_CM);

        n.getParam("/environment/apogee", target_apogee);

        n.getParam("/rocket/initial_speed", initial_speed);

        std::vector<float> diameter = {0, 0, 0};
        std::vector<float> length = {0, 0, 0};

        int nStage;

        n.getParam("/rocket/diameters", diameter);
        n.getParam("/rocket/stage_z", length);
        n.getParam("/rocket/stages", nStage);

        total_length = length[nStage - 1];

        surface[0] = diameter[1] * total_length;
        surface[1] = surface[0];
        surface[2] = diameter[1] * diameter[1] / 4 * 3.14159;

        float rho_air = 1.225;
        drag_coeff[0] = 0.5 * rho_air * surface[0] * Cd[0];
        drag_coeff[1] = 0.5 * rho_air * surface[1] * Cd[1];
        drag_coeff[2] = 0.5 * rho_air * surface[2] * Cd[2];

        total_Inertia[2] = dry_Inertia[2];

        update_CM(propellant_mass);

        n.getParam("/perturbation/acc_noise", acc_noise);
        n.getParam("/perturbation/acc_bias", acc_bias);

        n.getParam("/perturbation/gyro_noise", gyro_noise);
        n.getParam("/perturbation/gyro_bias", gyro_bias);

        n.getParam("/perturbation/baro_noise", baro_noise);
        n.getParam("/perturbation/baro_bias", baro_bias);

        n.getParam("/environment/ground_altitude", h0);

    }

    void dynamics_flight(const state &x,
                         state &xdot,
                         control &rocket_control,
                         control &aero_control,
                         control &perturbation_control,
                         const double &t) {
        // -------------- Simulation variables -----------------------------
        double g0 = 3.986e14 / pow(6371e3 + h0 + x(2), 2);  // Earth gravity in [m/s^2]

        double mass = dry_mass + x(13);                  // Instantaneous mass of the rocket in [kg]

        // Orientation of the rocket with quaternion
        Quaternion<double> attitude(x(9), x(6), x(7), x(8));
        attitude.normalize();
        Matrix<double, 3, 3> rot_matrix = attitude.toRotationMatrix();
        //std::cout << (180/3.14)*std::acos(x(9)*x(9) - x(6)*x(6) - x(7)*x(7) + x(8)*x(8)) << "\n";

        // Force in inertial frame: gravity
        Matrix<double, 3, 1> gravity;
        gravity << 0, 0, g0 * mass;

        // Total force in inertial frame [N]
        Matrix<double, 3, 1> total_force;
        total_force = rot_matrix * rocket_control.col(0) - gravity + aero_control.col(0) + perturbation_control.col(0);
        //std::cout << total_force.transpose() << "\n";

        // Angular velocity omega in quaternion format to compute quaternion derivative
        Quaternion<double> omega_quat(0.0, x(10), x(11), x(12));
        //std::cout << x.segment(10,3).transpose()*57.29 << "\n\n";

        // Tortal torque in body frame
        Matrix<double, 3, 1> I_inv;
        I_inv << 1 / total_Inertia[0], 1 / total_Inertia[1], 1 / total_Inertia[2];

        Matrix<double, 3, 1> total_torque;
        total_torque =
                rocket_control.col(1) + rot_matrix.transpose() * (aero_control.col(1) + perturbation_control.col(1));

        // -------------- Differential equation ---------------------

        // Position variation is speed
        xdot.head(3) = x.segment(3, 3);

        // Speed variation is Force/mass
        xdot.segment(3, 3) = total_force / mass;

        // Quaternion variation is 0.5*w◦q
        xdot.segment(6, 4) = 0.5 * (omega_quat * attitude).coeffs();

        // Angular speed variation is Torque/Inertia
        xdot.segment(10, 3) = rot_matrix * (total_torque.cwiseProduct(I_inv));

        // Mass variation is proportional to total thrust
        if (Isp != -1) {
            xdot(13) = -rocket_control.col(0).norm() / (Isp * g0);
        } else {
            xdot(13) = 0;
        }

        // Fake sensor data update -----------------
        sensor_acc = rot_matrix.transpose() * (total_force + gravity) / mass;

        sensor_gyro = rot_matrix.transpose() * x.segment(10, 3);

        sensor_baro = x(2);
    }

    void dynamics_rail(const state &x,
                       state &xdot,
                       control &rocket_control,
                       control &aero_control,
                       const double &t) {
        // -------------- Simulation variables -----------------------------
        double g0 = 3.986e14 / pow(6371e3 + x(2), 2);  // Earth gravity in [m/s^2]

        double mass = dry_mass + x(13);     // Instantaneous mass of the rocket in [kg]

        // Orientation of the rocket with quaternion
        Quaternion<double> attitude(x(9), x(6), x(7), x(8));
        attitude.normalize();
        Matrix<double, 3, 3> rot_matrix = attitude.toRotationMatrix();

        // Force in inertial frame: gravity
        Matrix<double, 3, 1> gravity;
        gravity << 0, 0, g0 * mass;

        // Total force in initial body frame [N] (rail frame)
        Matrix<double, 3, 1> total_force;
        total_force = rocket_control.col(0) - rot_matrix.transpose() * (gravity + aero_control.col(0));

        Matrix<double, 3, 1> body_acceleration;

        total_force.head(2) << 0.0, 0.0; // Zero force on axes perpendicular to rail to force rocket to stay on rail
        //std::cout << total_force << "\n";

        // Angular velocity omega in quaternion format to compute quaternion derivative
        Quaternion<double> omega_quat(0.0, x(10), x(11), x(12));

        // -------------- Differential equation ---------------------

        // Position variation is speed
        xdot.head(3) = x.segment(3, 3);

        // Speed variation is Force/mass
        xdot.segment(3, 3) = rot_matrix * total_force / mass;

        // Quaternion variation is zero to keep rail orientation
        xdot.segment(6, 4) << 0.5 * (omega_quat * attitude).coeffs();

        // Angular speed variation is zero to keep rail orientation
        xdot.segment(10, 3) << 0.0, 0.0, 0.0;

        // Mass variation is proportional to total thrust
        xdot.tail(1) << -rocket_control.col(0).norm() / (Isp * g0);

        // Fake sensor data update -----------------
        sensor_acc = (total_force + rot_matrix.transpose() * gravity) / mass;

        sensor_gyro << 0.0, 0.0, 0.0;

        sensor_baro = x(2);
    }
};


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <complex>
#include <boost/timer.hpp>

// Define only if you really have an Opteron!!!!!!!!!!!!!!!
// #define MTL_USE_OPTERON_OPTIMIZATION

#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;
using namespace mtl::recursion;
using namespace std;
using assign::plus_sum; using assign::assign_sum;

typedef bound_test_static<32>  test32_t;

// Use tiles of 2x4, usually works best on odin
// Any other tile sizes NxM can be tested with N * M <= 16
typedef gen_tiling_dmat_dmat_mult_t<2, 4, plus_sum>    tiling_mult_t;

// Recursive multiplication with 32x32 blocks and tiling in the blocks
typedef gen_recursive_dmat_dmat_mult_t<tiling_mult_t, test32_t>  recursive_mult_t;

// Assembler code if applicable, only implemented on certain bit masks
//   and only for double values (otherwise falls back on canonical impl. (slow))
typedef gen_platform_dmat_dmat_mult_t<plus_sum>     platform_mult_t;

// Recursive assembler version
typedef gen_recursive_dmat_dmat_mult_t<platform_mult_t, test32_t>  recursive_asm_mult_t;

int main(int argc, char* argv[])
{
    const int                                      n= 10;
    morton_dense<double,  doppled_32_row_mask>     A1(n, n), C1(n, n);
    morton_dense<double,  doppled_32_col_mask>     B1(n, n);
    recursive_mult_t                               m1;

    // fill matrix, e.g.
    hessian_setup(A1, 1.0);
    hessian_setup(B1, 1.0);

    // Recursive with fast 32x32 base case in C++
    m1(A1, B1, C1);
    cout << "Result with C++ is:\n" << C1 << "\n";

    // Now assembly code with shark teeth

    morton_dense<double,  shark_32_row_mask>     A2(n, n), C2(n, n);
    morton_dense<double,  shark_32_col_mask>     B2(n, n);
    recursive_asm_mult_t                         m2;

    // fill matrix, e.g.
    hessian_setup(A2, 1.0);
    hessian_setup(B2, 1.0);

    // Recursive with fast 32x32 base case in assembly
    m2(A2, B2, C2);
    cout << "Result with assembly is:\n" << C2 << "\n";

    return 0;
}


#include "math/sparse_matrix.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace GraphSfM {

SparseMatrix::SparseMatrix(const Eigen::SparseMatrix<double>& matrix)
    : matrix_(matrix) {}
SparseMatrix::~SparseMatrix() {}

// y = y + Ax;
void SparseMatrix::RightMultiply(const Eigen::VectorXd& x,
                                 Eigen::VectorXd* y) const {
  y->noalias() += matrix_ * x;
}
// y = y + A'x;
void SparseMatrix::LeftMultiply(const Eigen::VectorXd& x,
                                Eigen::VectorXd* y) const {
  y->noalias() += matrix_.transpose() * x;
}

int SparseMatrix::num_rows() const { return matrix_.rows(); };
int SparseMatrix::num_cols() const { return matrix_.cols(); };

}  // namespace GraphSfM


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

// TestDatum.h
#include <test/Helpers.h>

#include <vw/Cartography/Datum.h>
#include <vw/Core/Stopwatch.h>
#include <boost/assign/std/vector.hpp>

using namespace vw;
using namespace vw::cartography;
using namespace vw::test;

using namespace boost::assign;

vw::Vector3 vermille_2011_cart_to_geodetic( Datum const& d, Vector3 const& cart ) {
  const double a2 = d.semi_major_axis() * d.semi_major_axis();
  const double b2 = d.semi_minor_axis() * d.semi_minor_axis();
  const double e2 = 1 - b2 / a2;
  const double e4 = e2 * e2;

  double xy_dist = sqrt( cart[0] * cart[0] + cart[1] * cart[1] );
  double p = ( cart[0] * cart[0] + cart[1] * cart[1] ) / a2;
  double q = ( 1 - e2 ) * cart[2] * cart[2] / a2;
  double r = ( p + q - e4 ) / 6.0;
  double r3 = r * r * r;

  Vector3 llh;

  double evolute = 8 * r3 + e4 * p * q;
  double u = std::numeric_limits<double>::quiet_NaN();
  if ( evolute > 0 ) {
    // outside the evolute
    double right_inside_pow = sqrt(e4 * p * q);
    double sqrt_evolute = sqrt( evolute );
    u = r + 0.5 * pow(sqrt_evolute + right_inside_pow,2.0/3.0) +
      0.5 * pow(sqrt_evolute - right_inside_pow,2.0/3.0);
  } else if ( fabs(cart[2]) < std::numeric_limits<double>::epsilon() ) {
    // On the equator plane
    llh[1] = 0;
    llh[2] = norm_2( cart ) - d.semi_major_axis();
  } else if ( evolute < 0 and fabs(q) > std::numeric_limits<double>::epsilon() ) {
    // On or inside the evolute
    double atan_result = atan2( sqrt( e4 * p * q ), sqrt( -evolute ) + sqrt(-8 * r3) );
    u = -4 * r * sin( 2.0 / 3.0 * atan_result ) *
      cos( M_PI / 6.0 + 2.0 / 3.0 * atan_result );
  } else if ( fabs(q) < std::numeric_limits<double>::epsilon() and p <= e4 ) {
    // In the singular disc
    llh[2] = -d.semi_major_axis() * sqrt(1 - e2) * sqrt(e2 - p) / sqrt(e2);
    llh[1] = 2 * atan2( sqrt(e4 - p), sqrt(e2*(e2 - p)) + sqrt(1-e2) * sqrt(p) );
  } else {
    // Near the cusps of the evolute
    double inside_pow = sqrt(evolute) + sqrt(e4 * p * q);
    u = r + 0.5 * pow(inside_pow,2.0/3.0) +
      2 * r * r * pow(inside_pow,-2.0/3.0);
  }

  if (!std::isnan(u) ) {
    double v = sqrt( u * u + e4 * q );
    double u_v = u + v;
    double w = e2 * ( u_v - q ) / ( 2 * v );
    double k = u_v / ( w + sqrt( w * w + u_v ) );
    double D = k * xy_dist / ( k + e2 );
    double dist_2 = D * D + cart[2] * cart[2];
    llh[2] = ( k + e2 - 1 ) * sqrt( dist_2 ) / k;
    llh[1] = 2 * atan2( cart[2], sqrt( dist_2 ) + D );
  }

  if ( xy_dist + cart[0] > ( sqrt(2) - 1 ) * cart[1] ) {
    // Longitude is between -135 and 135
    llh[0] = 360.0 * atan2( cart[1], xy_dist + cart[0] ) / M_PI;
  } else if ( xy_dist + cart[1] < ( sqrt(2) + 1 ) * cart[0] ) {
    // Longitude is between -225 and 45
    llh[0] = - 90.0 + 360.0 * atan2( cart[0], xy_dist - cart[1] ) / M_PI;
  } else {
    // Longitude is between -45 and 225
    llh[0] = 90.0 - 360.0 * atan2( cart[0], xy_dist + cart[1] ) / M_PI;
  }
  llh[1] *= 180.0 / M_PI;

  return llh;
}

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
TEST( Datum, DatumDesc ) {
  Datum datum("NAD27");

  DatumDesc desc = datum.build_desc();

  Datum datum2(desc);

  EXPECT_EQ(datum.build_desc().DebugString(), datum2.build_desc().DebugString());
}
#endif

TEST( Datum, GeodeticConversion ) {
  Datum datum("WGS84");

  std::vector<double> values;
  values += -8000000.,-7000000.,-6500000.,-6100000.,-5788000.,-500000.,-100000.,-50000.,-10000.,-6000.,-1000.,-200.,-1.,0.,8000000.,7000000.,6500000.,6100000.,5788000.,500000.,100000.,50000.,10000.,6000.,1000.,200.,1;

  // The precision seems to be around a center when the vector is near
  // the center of the earth, which is the worse possible
  // situation. Things are better near the surface.
  for ( size_t ix = 0; ix < values.size(); ix++ ) {
    for ( size_t iy = 0; iy < values.size(); iy++ ) {
      for ( size_t iz = 0; iz < values.size(); iz++ ) {
        Vector3 test_xyz(values[ix],values[iy],values[iz]);
        EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(vermille_2011_cart_to_geodetic(datum,test_xyz)),
                            test_xyz, std::max(1e-12 * norm_2(test_xyz), 1e-2) );
      }
    }
  }

  EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(datum.cartesian_to_geodetic(Vector3(10000,10,-10))),
                      Vector3(10000,10,-10), 1e-6 );
  EXPECT_VECTOR_NEAR( datum.cartesian_to_geodetic(datum.geodetic_to_cartesian(Vector3(30,-10,173740))),
                      Vector3(30,-10,173740), 1e-6 );

  datum.set_well_known_datum("D_MOON"); // This is a spherical datum

  // The precision seems to be around a center when the vector is near
  // the center of the earth, which is the worse possible
  // situation. Things are better near the surface.
  for ( size_t ix = 0; ix < values.size(); ix++ ) {
    for ( size_t iy = 0; iy < values.size(); iy++ ) {
      for ( size_t iz = 0; iz < values.size(); iz++ ) {
        Vector3 test_xyz(values[ix],values[iy],values[iz]);
        EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(vermille_2011_cart_to_geodetic(datum,test_xyz)),
                            test_xyz, std::max(1e-12 * norm_2(test_xyz), 1e-2) );
      }
    }
  }

  EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(datum.cartesian_to_geodetic(Vector3(10000,10,-10))),
                      Vector3(10000,10,-10), 1e-6 );
  EXPECT_VECTOR_NEAR( datum.cartesian_to_geodetic(datum.geodetic_to_cartesian(Vector3(30,-10,173740))),
                      Vector3(30,-10,173740), 1e-6 );
}


// Created by Lina Felsner on Thu Aug 18th 2016

#ifndef __model_fdct_calibration_correction_hxx
#define __model_fdct_calibration_correction_hxx

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <LibProjectiveGeometry/ProjectionMatrix.h>
#include "Model.hxx"

#include <Utils/Projtable.hxx>

namespace Geometry {

	struct ModelFDCTCalibrationCorrection : public Model<7>
	{

		double pp_u;
		double pp_v;
		double spacing;
		double sid;
		double sdd;

		static std::vector<std::string> ParameterNames()
		{
			std::vector<std::string> names;
			// Set parameter names
			names.resize(size());
			names[0] = "Translation u";
			names[1] = "Translation v";
			names[2] = "Yaw";
			names[3] = "Pitch";
			names[4] = "Roll";
			names[5] = "Source Isocenter Distance";
			names[6] = "Source Detector Distance";
			return names;
		}

		enum ParameterSet { Identity, DetectorShifts, DetectorRigid2D, DetectorRotations, SDDandSID, All };
		static std::vector<std::string> ParameterSets()
		{
			std::vector<std::string> sets;
			sets[0] = "Identity";
			sets[1] = "DetectorShifts";
			sets[2] = "DetectorRigid2D";
			sets[4] = "DetectorRotations";
			sets[5] = "Source Detector and Iso-Center Distances";
			sets[6] = "All";
			return sets;
		}

		void computeMeanPPandSIDandSDD(const std::vector<Geometry::ProjectionMatrix> Ps)
		{
			std::vector<double> sids, sdds;
			ProjTable::ctCircularTrajectoryToParameters(Ps, spacing, 0x0, 0x0, 0x0, &sids, &sdds);

			// calculate mean
			double sumSID = std::accumulate(sids.begin(), sids.end(), 0.0);
			sid = sumSID / sids.size();
			double sumSDD = std::accumulate(sdds.begin(), sdds.end(), 0.0);
			sdd = sumSDD / sdds.size();

			std::cout << "Mean sid and sdd: " << sid << " " << sdd << std::endl;

			Geometry::RP2Point ppmean(0,0,0);
			for (int i = 0; i < (int)Ps.size(); i++)
			{
				Geometry::RP2Point pp=Ps[i].block<3, 3>(0, 0)*Ps[i].block<1, 3>(2, 0).transpose();
				pp = pp / pp(2);
				ppmean+=pp;
			}
			pp_u = ppmean[0] / ppmean(2);
			pp_v = ppmean[1] / ppmean(2);
			std::cout << "Mean principal point: " << pp_u << " " << pp_v << std::endl;
		}

		ModelFDCTCalibrationCorrection(int _nu, int _nv, double  _spacing, double sid, double sdd, ParameterSet active = Identity)
			: Model<7>(ParameterNames())
			, pp_u(0.5*_nu)
			, pp_v(0.5*_nv)
			, spacing(_spacing)
		{
			// Set parameter names
			names = ModelFDCTCalibrationCorrection::ParameterNames();
			// Set active parameters
			setActiveParameters(active);
		}

		ModelFDCTCalibrationCorrection(double  _spacing, const std::vector<Geometry::ProjectionMatrix>& Ps, ParameterSet active = Identity)
			: Model<7>(ParameterNames())
			, spacing(_spacing)
		{
			// Set parameter names
			names = ModelFDCTCalibrationCorrection::ParameterNames();
			// Set active parameters
			setActiveParameters(active);
			// Analyze geometry
			computeMeanPPandSIDandSDD(Ps);
		}

		/// Several common sets of active parameters
		void setActiveParameters(ParameterSet set)
		{
			for (int i = 0; i<size(); i++) active[i] = (set==All);

			switch (set)
			{
				default:
				case Identity:
				case All:
					break;
				case DetectorShifts:
					active[0] = active[1] = true;
				break;
				case SDDandSID:
					active[5] = active[6] = true;
				break;
				case DetectorRotations:
					active[2]=active[3]=active[4]=true;
				break;
				case DetectorRigid2D:
					active[0] =active[1] = active[4] = true;
				break;
			}
		}

		// Transform the Projection Matrix via: H*H_init*P*T
		void transform(std::vector<Geometry::ProjectionMatrix> &Ps, Eigen::Matrix3d H_init = Eigen::Matrix3d::Identity()) const
		{

			//TODO
			std::pair<Eigen::Matrix3d, Eigen::Matrix4d> HT = getTransforms(Ps);
			Eigen::Matrix3d H = HT.first;
			Eigen::Matrix4d T = HT.second;

			for (int i = 0; i < Ps.size(); i++)
			{
				Ps[i] = H*H_init*Ps[i]*T;
				Geometry::normalizeProjectionMatrix(Ps[i]);
			}
		}

		// Compose a homography from Parametrization
		std::pair<Eigen::Matrix3d, Eigen::Matrix4d> getTransforms(std::vector<Geometry::ProjectionMatrix> Ps) const
		{
			const double *x(param); //parameter vector

			Eigen::Matrix3d H = Eigen::Matrix3d::Identity();
			Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

			Geometry::RP2Point ppmean(pp_u, pp_v, 1.0);

			double sid_scale = (sid + x[5]) / sid;
			double sdd_scale = (sdd + x[6]) / sdd;

//// H = Matrix 3x3
			Eigen::Matrix3d H_roll, H_shift, H_scale;
			H_roll = H_shift = H_scale = H;

			// using the principal point / center of the image
			auto Hpp = H, Hppinv = H;
			Hpp.block<3, 1>(0, 2) = ppmean;
			Hppinv(0, 2) = -ppmean[0];
			Hppinv(1, 2) = -ppmean[1];

			//Roll -> Rotation
			if (x[4] != 0)
			{
				H_roll <<
					+cos(x[4]), -sin(x[4]), 0,
					+sin(x[4]), +cos(x[4]), 0,
					0,			0,			1;
			}

			// Translation u and Yaw
			H_shift(0, 2) += x[0];
			H_shift(0, 2) += tan(x[2])*sdd;

			// Translation v and Pitch
			H_shift(1, 2) += x[1];
			H_shift(1, 2) += tan(x[3])*sdd;

			//SDD
			H_scale.block<2, 2>(0, 0) *= sdd_scale;

			H = H_shift * Hpp * H_roll * H_scale * Hppinv;

//// T = Matrix 4x4
			//SID
			T.block<3, 3>(0, 0) *= sid_scale;

//// HT
			std::pair<Eigen::Matrix3d, Eigen::Matrix4d> HT = { H, T };
			return HT;
		}

	};

}

#endif // __model_fdct_calibration_correction_hxx


#pragma once

// system includes ---------------------------------------------------------
#include <fstream>
#include <iostream>

#include <boost/static_assert.hpp>
#include <tuple>

#include <regex>

// own includes ------------------------------------------------------------
#include "enum/enum.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"
#include "spectral_basis.hpp"
#include "spectral_basis_factory_base.hpp"
#include "spectral_elem.hpp"
#include "spectral_elem_accessor.hpp"
#include "spectral_function.hpp"

namespace boltzmann {

namespace local_ {
// ----------------------------------------------------------------------
// Hermite basis
class HermiteHX : public HermiteH
{
 public:
  HermiteHX(int k, double w = 0.5)
      : HermiteH(k, w)
  {
  }
  HermiteHX() {}
};

class HermiteHY : public HermiteH
{
 public:
  HermiteHY(int k, double w = 0.5)
      : HermiteH(k, w)
  {
  }
  HermiteHY() {}
};
}  // end namespace local_

// ----------------------------------------------------------------------
class SpectralBasisFactoryHN : public SpectralBasisFactoryBase<local_::HermiteHX, local_::HermiteHY>
{
 public:
  typedef local_::HermiteHX hx_t;
  typedef local_::HermiteHY hy_t;

 public:
  /// definition of element ordering, TODO: define this at a global place
  /**
   *
   *
   * @param basis
   * @param K
   * @param L
   * @param beta
   * @param sorted sort basis functions by `l`, the angular index
   */
  static void create(basis_type& basis, int K, double beta = 2);

  static void write_basis_descriptor(const basis_type& basis,
                                     std::string fname = "spectral_basis.desc");

  /**
   * @brief read basis_descriptor file
   *
   * @param basis
   * @param descriptor_file
   */
  static void create(basis_type& basis, std::string descriptor_file);

  static elem_t make_single_elem(int kx, int ky, double w = 0.5);
};

// ----------------------------------------------------------------------
inline void
SpectralBasisFactoryHN::create(basis_type& basis, int K, double beta)
{
  for (int k = 0; k < K; ++k) {
    for (int s = 0; s <= k; ++s) {
      local_::HermiteHX hx(s, 1 / beta);
      local_::HermiteHY hy(k - s, 1 / beta);
      basis.add_elem(hx, hy);
    }
  }

  basis.finalize();
}

// ----------------------------------------------------------------------
inline void
SpectralBasisFactoryHN::write_basis_descriptor(const basis_type& basis, std::string fname)
{
  // typename elem_t::Acc::template get<fa_type> xir_getter;
  // typename elem_t::Acc::template get<fr_type> rr_getter;

  typename elem_t::Acc::get<fa_type> xir_getter;
  typename elem_t::Acc::get<fr_type> rr_getter;

  std::ofstream fout(fname);
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    fout << xir_getter(*it).get_id() << "\t" << rr_getter(*it).get_id() << std::endl;
  }
  fout.close();
}

// ----------------------------------------------------------------------
inline SpectralBasisFactoryHN::elem_t
SpectralBasisFactoryHN::make_single_elem(int kx, int ky, double w)
{
  local_::HermiteHX hx(kx, w);
  local_::HermiteHY hy(ky, w);
  return elem_t(hx, hy);
}

// ----------------------------------------------------------------------
inline void
SpectralBasisFactoryHN::create(basis_type& basis, std::string descriptor_file)
{
  (void)basis;
  (void)descriptor_file;
  throw std::runtime_error("SpectralBasisFactoryHN::create is not implemented");
}

}  // end namespace boltzmann


// UltimateMinmax.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/punctuation/paren.hpp>
#include <boost/preprocessor/repetition/enum_binary_params.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>

#include "arithmetic_type_promotion.hpp"

#define MAX_MINMAX_ARITY 3

namespace std {

#define BASE_FUNCTION(func, op) \
	template <typename A, typename B> \
	typename boost::enable_if<boost::mpl::and_<boost::is_arithmetic<A>, boost::is_arithmetic<B>, boost::mpl::not_<boost::is_same<A, B>>> \
		, typename boost::arithmetic_type_promotion<A, B>::type>::type \
	func(A a, B b) \
	{ \
		typedef typename boost::arithmetic_type_promotion<A, B>::type R; \
		return R(a) op R(b) ? R(a) : R(b); \
	}

BASE_FUNCTION(min, <)
BASE_FUNCTION(max, >)

#define FUNCTION_CALL_PREFIX(z, n, data) data(BOOST_PP_CAT(t, n),
#define FUNCTION_CALL_SUFFIX(z, n, data) BOOST_PP_RPAREN()
#define ENABLE_IF_CONDITION_PREFIX(z, n, data) boost::mpl::and_<boost::is_arithmetic<BOOST_PP_CAT(T, n)>,
#define ENABLE_IF_CONDITION_SUFFIX(z, n, data) >
#define TYPE_PROMOTION_PREFIX(z, n, data) typename boost::arithmetic_type_promotion<BOOST_PP_CAT(T, n),
#define TYPE_PROMOTION_SUFFIX(z, n, data) >::type

#define PREFIX_SUFFIX(n, data, prefix, middle, suffix) \
	BOOST_PP_REPEAT(n, prefix, data) \
	middle \
	BOOST_PP_REPEAT(n, suffix, data)

#define FUNCTION_TEMPLATE(z, n, function_name) \
template <BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n), typename T)> \
typename boost::enable_if<PREFIX_SUFFIX(n, ~, ENABLE_IF_CONDITION_PREFIX, boost::is_integral<BOOST_PP_CAT(T, n)>, ENABLE_IF_CONDITION_SUFFIX) \
	, PREFIX_SUFFIX(n, ~, TYPE_PROMOTION_PREFIX, BOOST_PP_CAT(T, n), TYPE_PROMOTION_SUFFIX) \
>::type \
function_name(BOOST_PP_ENUM_BINARY_PARAMS(BOOST_PP_INC(n), T, t)) \
{ \
	return PREFIX_SUFFIX(n, function_name, FUNCTION_CALL_PREFIX, BOOST_PP_CAT(t, n), FUNCTION_CALL_SUFFIX) \
	; \
}

#define FUNCTION_TEMPLATE_DEC_N(z, n, data) FUNCTION_TEMPLATE(z, BOOST_PP_DEC(n), data)

BOOST_PP_REPEAT_FROM_TO(3, BOOST_PP_INC(MAX_MINMAX_ARITY), FUNCTION_TEMPLATE_DEC_N, min)
BOOST_PP_REPEAT_FROM_TO(3, BOOST_PP_INC(MAX_MINMAX_ARITY), FUNCTION_TEMPLATE_DEC_N, max)

}

#include <typeinfo>

template <class T>
void type_discovery(T t)
{
	cout << typeid(T).name() << endl;
}

int _tmain(int argc, _TCHAR* argv[])
{
	int a = max(char(1), short(2));
	int x = max(5, 6);
	double b = max(int(2), 3.0, 1);
	int c = max(2, 1, 3, 4);
	int d = max(2, 3, 4, 1, 5);

	cout << a << b << c << d << endl;

	return 0;
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define FUSION_MAX_VECTOR_SIZE 20
#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/include/vector.hpp>

#include <limbo/limbo.hpp>

using namespace limbo;

struct Params {

    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 0.001);
    };

    struct kernel_exp {
        BO_PARAM(double, sigma_sq, 1);
        BO_PARAM(double, l, 1);
    };

    struct kernel_maternthreehalves {
        BO_PARAM(double, sigma_sq, 1);
        BO_PARAM(double, l, 0.2);
    };

    struct kernel_maternfivehalves {
        BO_PARAM(double, sigma_sq, 1);
        BO_PARAM(double, l, 0.2);
    };

    struct mean_constant {
        BO_PARAM(double, constant, 0);
    };

    struct acqui_ucb : public defaults::acqui_ucb {
    };

    struct acqui_gpucb : public defaults::acqui_gpucb {
    };

    struct opt_gridsearch {
        BO_PARAM(int, bins, 20);
    };
#ifdef USE_LIBCMAES
    struct opt_cmaes : public defaults::opt_cmaes {
    };
#endif
#ifdef USE_NLOPT
    struct opt_nloptnograd : public defaults::opt_nloptnograd {
    };
#endif
    struct opt_rprop : public defaults::opt_rprop {
    };

    struct opt_parallelrepeater : public defaults::opt_parallelrepeater {
    };

    struct init_gridsampling {
        BO_PARAM(int, bins, 5);
    };

    struct init_randomsampling {
        BO_PARAM(int, samples, 5);
    };

    struct init_randomsamplinggrid {
        BO_PARAM(int, samples, 5);
        BO_PARAM(int, bins, 5);
    };

    struct stop_maxiterations {
        BO_PARAM(int, iterations, 20);
    };

    struct stop_maxpredictedvalue {
        BO_PARAM(double, ratio, 2);
    };

    struct bayes_opt_bobase : public defaults::bayes_opt_bobase {
        BO_PARAM(bool, stats_enabled, true);
    };

    struct bayes_opt_boptimizer : public defaults::bayes_opt_boptimizer {
    };
};

struct MeanEval {
    MeanEval(size_t dim_out = 1) {}

    template <typename GP>
    Eigen::VectorXd operator()(const Eigen::VectorXd& x, const GP&) const
    {
        Eigen::VectorXd res(2);
        res(0) = 2.5 * x(0);
        res(1) = -4.5 * x(1);
        return res;
    }
};

struct Average {
    using result_type = double;
    double operator()(const Eigen::VectorXd& x) const
    {
        return (x(0) + x(1)) / 2;
    }
};

struct StateEval {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, 2);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::VectorXd res(2);
        res(0) = 3 * x(0) + 5;
        res(1) = -5 * x(1) + 2;
        return res;
    }
};

int main()
{
    // clang-format off
    @declarations
    @optimizer.optimize(StateEval());
    @optimizer.best_observation();
    @optimizer.best_sample();
    @optimizer.optimize(StateEval(), Average(), true);
    @optimizer.best_observation(Average());
    @optimizer.best_sample(Average());
    // clang-format on
}


//
//  PardisoSolver.h
//

#ifndef _PardisoSolver__
#define _PardisoSolver__

#include "LinSysSolver.hpp"

#include <vector>
#include <set>
#include <Eigen/Core>
#include <Eigen/Sparse>

namespace OptCuts {

    //extract II,JJ,SS (row,column and value vectors) from sparse matrix, Eigen version
    //Olga Diamanti's method for PARDISO
    void extract_ij_from_matrix(const Eigen::SparseMatrix<double> &A,
                                Eigen::VectorXi &II,
                                Eigen::VectorXi &JJ,
                                Eigen::VectorXd &SS);

    //extract II,JJ,SS (row,column and value vectors) from sparse matrix, std::vector version
    void extract_ij_from_matrix(const Eigen::SparseMatrix<double> &A,
                                std::vector<int> &II,
                                std::vector<int> &JJ,
                                std::vector<double> &SS);

    extern "C" {
        /* PARDISO prototype. */
        void pardisoinit (void   *, int    *,   int *, int *, double *, int *);
        void pardiso     (void   *, int    *,   int *, int *,    int *, int *,
                          double *, int    *,    int *, int *,   int *, int *,
                          int *, double *, double *, int *, double *);
        void pardiso_chkmatrix  (int *, int *, double *, int *, int *, int *);
        void pardiso_chkvec     (int *, int *, double *, int *);
        void pardiso_printstats (int *, int *, double *, int *, int *, int *,
                                 double *, int *);
    }

    template <typename vectorTypeI, typename vectorTypeS>
    class PardisoSolver : public LinSysSolver<vectorTypeI, vectorTypeS>
    {
        typedef LinSysSolver<vectorTypeI, vectorTypeS> Base;

    public:
        PardisoSolver() ;
        ~PardisoSolver();

        void set_type(int threadAmt, int _mtype, bool is_upper_half = false);

        void init(int threadAmt);

        void set_pattern(const vectorTypeI &II,
                         const vectorTypeI &JJ,
                         const vectorTypeS &SS);
        void set_pattern(const std::vector<std::set<int>>& vNeighbor,
                         const std::set<int>& fixedVert);
        void set_pattern(const Eigen::SparseMatrix<double>& mtr);

        void analyze_pattern();

        bool factorize();

        void solve(Eigen::VectorXd &rhs,
                   Eigen::VectorXd &result);

        void update_a(const vectorTypeS &SS);
        void update_a(const vectorTypeI &II,
                      const vectorTypeI &JJ,
                      const vectorTypeS &SS);

    protected:
        //vector that indicates which of the elements II,JJ input will be
        //kept and read into the matrix (for symmetric matrices, only those
        //elements II[i],JJ[i] for which II[i]<<JJ[i] will be kept)
        std::vector<int> lower_triangular_ind;

        std::vector<Eigen::VectorXi> iis;

        //pardiso stuff
        /*
         1: real and structurally symmetric, supernode pivoting
         2: real and symmetric positive definite
         -2: real and symmetric indefinite, diagonal or Bunch-Kaufman pivoting
         11: real and nonsymmetric, complete supernode pivoting
         */
        int mtype;       /* Matrix Type */

        // Remember if matrix is symmetric or not, to
        // decide whether to eliminate the non-upper-
        // diagonal entries from the input II,JJ,SS
        bool is_symmetric;
        bool is_upper_half;

        int nrhs = 1;     /* Number of right hand sides. */
        /* Internal solver memory pointer pt, */
        /* 32-bit: int pt[64]; 64-bit: long int pt[64] */
        /* or void *pt[64] should be OK on both architectures */
        void *pt[64];
        /* Pardiso control parameters. */
        int iparm[64];
        double   dparm[64];
        int maxfct, mnum, phase, error, msglvl, solver =0;
        /* Number of processors. */
        int      num_procs;
        /* Auxiliary variables. */
        char    *var;
        int i, k;
        double ddum;          /* Double dummy */
        int idum;         /* Integer dummy. */
    };

}

#endif /* defined(_PardisoSolver__) */


#include <stan/math/prim/arr.hpp>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <vector>

TEST(MetaTraits, size_of) {
  using stan::size_of;

  std::vector<double> x2(3);
  EXPECT_EQ(3U, size_of(x2));

  std::vector<std::vector<double>> x3(6);
  EXPECT_EQ(6U, size_of(x3));

  std::vector<Eigen::MatrixXd> x4(8);
  EXPECT_EQ(8U, size_of(x4));
}