/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_ALGO_NN_LAYER_CONV_HPP
#define SP_ALGO_NN_LAYER_CONV_HPP

#include <boost/assert.hpp>

#include "layer.hpp"
#include "connectivity.hpp"
#include "detail/layers.hpp"
#include "sp/util/types.hpp"
#include "params.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \brief Convolutional Layer implementation
 *
 * Convolves input by a set of feature kernels and returns the convolution
 * results
 *
 * Note: This convolution implementation does not rotate the kernel
 *       and is therefore not convolution but cross correlation, mathematically
 *       speaking.
 *
 * Summary of parameters (Soft and Hard, i.e. parameterized and derived)
 *  - N - Number of images in a mini-batch
 *  - C - The number of input feature maps
 *  - H - Height of input image
 *  - W - Width of input image
 *  - K - Number of output feature maps (number of kernels)
 *  - R - Height of filter kernel
 *  - S - Width of filter kernel
 *  - U - Vertical Stride
 *  - V - Horizontal Stride
 *  - pad_h - Height of zero-padding
 *  - pad_w - Width of zero-padding
 *
 * Output O of is a four dimensional tensor in R^NKPQ,
 *  where ```P = f(H, R, u, pad_h) and Q = f(W, S, v, pad_w)```
 *  where
 *  *   ```f(H, R, u, pad_h) = ceil((H - R + 1 + 2 * pad_h) / 2)```
 *  *   ```f(W, S, v, pad_w) = ceil((W - S + 1 + 2 * pad_w) / 2)```
 *
 * Convolution Modes (common LA terms, matlab, octave)
 *  - valid - ```pad_w = pad_h = 0```
 *  - same  - ```pad_h = R/2 and pad_w = S / 2```
 *  - full  - ```pad_h = R - 1, pad_w = S - 1```
 *
 * \tparam InputDim, includes:
 * - Width
 * - Height
 * - Depth (or channels)
 * \tparam KernelDim, includes:... tbd
 * \tparam Biased (optional) default true. Whether or not the layer contains bias.
 * \tparam Sparsity The sparsity object. See #group_sparsity and #no_sparsity
 */
template<
    typename InputVolume,
    typename KernelParams = kernel_params_default,
    bool Biased = true,
    typename Connectivity = full_connectivity,
    size_t Dilation = 0
>
struct conv_layer : layer<
    InputVolume,
    detail::convolution_kernel_out_dims_t<InputVolume, KernelParams>,
    conv_layer<InputVolume, KernelParams, Biased, Connectivity, Dilation>
> {

    /**
     * Base type
     */
    using base = layer<
        InputVolume,
        detail::convolution_kernel_out_dims_t<InputVolume, KernelParams>,
        conv_layer<InputVolume, KernelParams, Biased, Connectivity, Dilation>
    >;

    /**
     * Validates KernelParams type
     */
    static_assert(util::is_instantiation_of_v<KernelParams, kernel_params>, "KernelParams template parameter must be an instance of kernel_params");

    /**
     * Kernel parameters
     */
    using kernel_params = KernelParams;

    /**
     * Connectivity type (full, ngroups, etc)
     */
    using connectivity_type = Connectivity;

    /**
     * \brief Dilation parameter
     * \todo TBD: https://arxiv.org/abs/1511.07122
     */
    static_assert(Dilation == 0, "Dilation is not implemented");

    /**
     * Whether or not this layer has biased
     */
    constexpr static bool biased = Biased;

    /**
     * \brief Input Dimensions
     */
    using input_dims = typename base::input_dims;

    /**
     * \brief Output Dimensions
     */
    using output_dims = typename base::output_dims;

    /**
     * \brief Weight dimensions
     */
    using weights_dims = weight_dims<
        output_dims::d,
        input_dims::d,
        kernel_params::h,
        kernel_params::w
    >;

    void forward_prop_impl(tensor_4& input, tensor_4& output) {

        /**
         * Number of samples in the input
         */
        const size_t samples = input.dimension(0);

        /**
         * Perform  forward propagation for every sample
         */
        #pragma omp parallel for simd
        for (size_t si = 0; si < samples; ++si) {
            /**
             * Loop over the pairs of (D_out, D_in), i.e. input_dims::d*output_dims::d
             */
            for (size_t od = 0; od < output_dims::d; ++od) {
                for (size_t id = 0; id < input_dims::d; ++id) {
                    if(connections(od, id)) {
                        /*
                         * If the output channel is connected to the input channel,
                         * then perform convolution. This is done to support limited
                         * connectivity when required
                         *
                         * Then, perform the convolution op
                         *
                         * \todo add dilation
                         * \todo Optimize for smaller kernels (common, 2x2, 3x3, etc)
                         */
                        for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                            for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                float_t sum = 0;
                                for (size_t ky = 0; ky < kernel_params::h; ++ky) {
                                    for (size_t kx = 0; kx < kernel_params::w; ++kx) {
                                        auto& in_val = input(si, id, iny+ky, inx+kx);
                                        auto& w_val = w(od, id, ky, kx);
                                        sum += in_val * w_val;
                                    }
                                }
                                output(si, od, oy, ox) += sum;
                            }
                        }
                    }
                }
                if constexpr(biased) {
                    /**
                     * Add bias to every output vector the depth slice at output(od)
                     */
                    output.chip(si, 0).chip(od, 0) = output.chip(si, 0).chip(od, 0) + b(od);
                }
            }
        }
    }

    /**
     * \brief Back propagation implementation
     *
     * \todo Optimize
     */
    void backward_prop_impl(    tensor_4& prev_out,
                                tensor_4& prev_delta,
                                tensor_4& curr_out,
                                tensor_4& curr_delta)  {

        /**
         * Number of samples in the previous output
         */
        const size_t samples = prev_out.dimension(0);

        /**
         * Perform back propagation
         *
         * For every sample in the input
         */
        #pragma omp parallel for simd
        for(size_t si = 0; si < samples; ++si) {
            /**
             * For every (input depth, output depth) pair that is connected
             */
            for (size_t id = 0; id < input_dims::d; ++id) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    if(connections(od, id)) {
                        /* Propagate the current delta to the previous delta through the kernel */
                        for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                            for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                float_t& grad = curr_delta(si, od, oy, ox);
                                for (size_t wy = 0; wy < weights_dims::h; ++wy) {
                                    for (size_t wx = 0; wx < weights_dims::w; ++wx) {
                                        auto& w_val = w(od, id, wy, wx);
                                        prev_delta(si, id, iny + wy, inx + wx) += w_val * grad;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            for (size_t id = 0; id < input_dims::d; ++id) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    if(connections(od, id)) {
                        for (size_t wy = 0; wy < weights_dims::h; ++wy) {
                            for (size_t wx = 0; wx < weights_dims::w; ++wx) {
                                float_t delta = 0;
                                for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                                    for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                        auto& po = prev_out(si, id, oy + wy, ox + wx);
                                        auto& cd = curr_delta(si, od, oy, ox);
                                        delta +=  po * cd;
                                    }
                                }
                                dw(si, od, id, wy, wx) += delta;
                            }
                        }
                    }
                }
            }
            if (biased) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    tensor_0 sum = curr_delta.chip(si, 0).chip(od, 0).sum();
                    db(si, od) += sum(0);
                }
            }
        }
    }

    /**
     * \brief Weights of the layer.
     */
    weights_type w;

    /**
     * Weights Delta
     */
    weights_delta_type dw;

    /**
     * \brief Bias.
     */
    bias_type b;

    /**
     * Bias Delta.
     */
    bias_delta_type db;

    connectivity_type connections;

};

SP_ALGO_GEN_NAMESPACE_END

#endif	/* SP_ALGO_NN_LAYER_CONV_HPP */


//  (C) Copyright Nick Thompson 2019.
//  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_DIFFERENTIATION_LANCZOS_SMOOTHING_HPP
#define BOOST_MATH_DIFFERENTIATION_LANCZOS_SMOOTHING_HPP
#include <cmath> // for std::abs
#include <cstddef>
#include <limits> // to nan initialize
#include <vector>
#include <string>
#include <stdexcept>
#include <type_traits>
#include <boost/math/tools/assert.hpp>

namespace boost::math::differentiation {

namespace detail {
template <typename Real>
class discrete_legendre {
  public:
    explicit discrete_legendre(std::size_t n, Real x) : m_n{n}, m_r{2}, m_x{x},
                                                        m_qrm2{1}, m_qrm1{x},
                                                        m_qrm2p{0}, m_qrm1p{1},
                                                        m_qrm2pp{0}, m_qrm1pp{0}
    {
        using std::abs;
        BOOST_MATH_ASSERT_MSG(abs(m_x) <= 1, "Three term recurrence is stable only for |x| <=1.");
        // The integer n indexes a family of discrete Legendre polynomials indexed by k <= 2*n
    }

    Real norm_sq(int r) const
    {
        Real prod = Real(2) / Real(2 * r + 1);
        for (int k = -r; k <= r; ++k) {
            prod *= Real(2 * m_n + 1 + k) / Real(2 * m_n);
        }
        return prod;
    }

    Real next()
    {
        Real N = 2 * m_n + 1;
        Real num = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) * m_qrm2;
        Real tmp = (2 * m_r - 1) * m_x * m_qrm1 - num / Real(4 * m_n * m_n);
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp / m_r;
        ++m_r;
        return m_qrm1;
    }

    Real next_prime()
    {
        Real N = 2 * m_n + 1;
        Real s = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) / Real(4 * m_n * m_n);
        Real tmp1 = ((2 * m_r - 1) * m_x * m_qrm1 - s * m_qrm2) / m_r;
        Real tmp2 = ((2 * m_r - 1) * (m_qrm1 + m_x * m_qrm1p) - s * m_qrm2p) / m_r;
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp1;
        m_qrm2p = m_qrm1p;
        m_qrm1p = tmp2;
        ++m_r;
        return m_qrm1p;
    }

    Real next_dbl_prime()
    {
        Real N = 2*m_n + 1;
        Real trm1 = 2*m_r - 1;
        Real s = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) / Real(4 * m_n * m_n);
        Real rqrpp = 2*trm1*m_qrm1p + trm1*m_x*m_qrm1pp - s*m_qrm2pp;
        Real tmp1 = ((2 * m_r - 1) * m_x * m_qrm1 - s * m_qrm2) / m_r;
        Real tmp2 = ((2 * m_r - 1) * (m_qrm1 + m_x * m_qrm1p) - s * m_qrm2p) / m_r;
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp1;
        m_qrm2p = m_qrm1p;
        m_qrm1p = tmp2;
        m_qrm2pp = m_qrm1pp;
        m_qrm1pp = rqrpp/m_r;
        ++m_r;
        return m_qrm1pp;
    }

    Real operator()(Real x, std::size_t k)
    {
        BOOST_MATH_ASSERT_MSG(k <= 2 * m_n, "r <= 2n is required.");
        if (k == 0)
        {
            return 1;
        }
        if (k == 1)
        {
            return x;
        }
        Real qrm2 = 1;
        Real qrm1 = x;
        Real N = 2 * m_n + 1;
        for (std::size_t r = 2; r <= k; ++r) {
            Real num = (r - 1) * (N * N - (r - 1) * (r - 1)) * qrm2;
            Real tmp = (2 * r - 1) * x * qrm1 - num / Real(4 * m_n * m_n);
            qrm2 = qrm1;
            qrm1 = tmp / r;
        }
        return qrm1;
    }

    Real prime(Real x, std::size_t k) {
        BOOST_MATH_ASSERT_MSG(k <= 2 * m_n, "r <= 2n is required.");
        if (k == 0) {
            return 0;
        }
        if (k == 1) {
            return 1;
        }
        Real qrm2 = 1;
        Real qrm1 = x;
        Real qrm2p = 0;
        Real qrm1p = 1;
        Real N = 2 * m_n + 1;
        for (std::size_t r = 2; r <= k; ++r) {
            Real s =
                (r - 1) * (N * N - (r - 1) * (r - 1)) / Real(4 * m_n * m_n);
            Real tmp1 = ((2 * r - 1) * x * qrm1 - s * qrm2) / r;
            Real tmp2 = ((2 * r - 1) * (qrm1 + x * qrm1p) - s * qrm2p) / r;
            qrm2 = qrm1;
            qrm1 = tmp1;
            qrm2p = qrm1p;
            qrm1p = tmp2;
        }
        return qrm1p;
    }

  private:
    std::size_t m_n;
    std::size_t m_r;
    Real m_x;
    Real m_qrm2;
    Real m_qrm1;
    Real m_qrm2p;
    Real m_qrm1p;
    Real m_qrm2pp;
    Real m_qrm1pp;
};

template <class Real>
std::vector<Real> interior_velocity_filter(std::size_t n, std::size_t p) {
    auto dlp = discrete_legendre<Real>(n, 0);
    std::vector<Real> coeffs(p+1);
    coeffs[1] = 1/dlp.norm_sq(1);
    for (std::size_t l = 3; l < p + 1; l += 2)
    {
        dlp.next_prime();
        coeffs[l] = dlp.next_prime()/ dlp.norm_sq(l);
    }

    // We could make the filter length n, as f[0] = 0,
    // but that'd make the indexing awkward when applying the filter.
    std::vector<Real> f(n + 1);
    // This value should never be read, but this is the correct value *if it is read*.
    // Hmm, should it be a nan then? I'm not gonna agonize.
    f[0] = 0;
    for (std::size_t j = 1; j < f.size(); ++j)
    {
        Real arg = Real(j) / Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        f[j] = coeffs[1]*arg;
        for (std::size_t l = 3; l <= p; l += 2)
        {
            dlp.next();
            f[j] += coeffs[l]*dlp.next();
        }
        f[j] /= (n * n);
    }
    return f;
}

template <class Real>
std::vector<Real> boundary_velocity_filter(std::size_t n, std::size_t p, int64_t s)
{
    std::vector<Real> coeffs(p+1, std::numeric_limits<Real>::quiet_NaN());
    Real sn = Real(s) / Real(n);
    auto dlp = discrete_legendre<Real>(n, sn);
    coeffs[0] = 0;
    coeffs[1] = 1/dlp.norm_sq(1);
    for (std::size_t l = 2; l < p + 1; ++l)
    {
        // Calculation of the norms is common to all filters,
        // so it seems like an obvious optimization target.
        // I tried this: The spent in computing the norms time is not negligible,
        // but still a small fraction of the total compute time.
        // Hence I'm not refactoring out these norm calculations.
        coeffs[l] = dlp.next_prime()/ dlp.norm_sq(l);
    }

    std::vector<Real> f(2*n + 1);
    for (std::size_t k = 0; k < f.size(); ++k)
    {
        Real j = Real(k) - Real(n);
        Real arg = j/Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        f[k] = coeffs[1]*arg;
        for (std::size_t l = 2; l <= p; ++l)
        {
            f[k] += coeffs[l]*dlp.next();
        }
        f[k] /= (n * n);
    }
    return f;
}

template <class Real>
std::vector<Real> acceleration_filter(std::size_t n, std::size_t p, int64_t s)
{
    BOOST_MATH_ASSERT_MSG(p <= 2*n, "Approximation order must be <= 2*n");
    BOOST_MATH_ASSERT_MSG(p > 2, "Approximation order must be > 2");

    std::vector<Real> coeffs(p+1, std::numeric_limits<Real>::quiet_NaN());
    Real sn = Real(s) / Real(n);
    auto dlp = discrete_legendre<Real>(n, sn);
    coeffs[0] = 0;
    coeffs[1] = 0;
    for (std::size_t l = 2; l < p + 1; ++l)
    {
        coeffs[l] = dlp.next_dbl_prime()/ dlp.norm_sq(l);
    }

    std::vector<Real> f(2*n + 1, 0);
    for (std::size_t k = 0; k < f.size(); ++k)
    {
        Real j = Real(k) - Real(n);
        Real arg = j/Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        for (std::size_t l = 2; l <= p; ++l)
        {
            f[k] += coeffs[l]*dlp.next();
        }
        f[k] /= (n * n * n);
    }
    return f;
}

} // namespace detail

template <typename Real, std::size_t order = 1>
class discrete_lanczos_derivative {
public:
    discrete_lanczos_derivative(Real const & spacing,
                                std::size_t n = 18,
                                std::size_t approximation_order = 3)
        : m_dt{spacing}
    {
        static_assert(!std::is_integral_v<Real>,
                      "Spacing must be a floating point type.");
        BOOST_MATH_ASSERT_MSG(spacing > 0,
                         "Spacing between samples must be > 0.");

        if constexpr (order == 1)
        {
            BOOST_MATH_ASSERT_MSG(approximation_order <= 2 * n,
                             "The approximation order must be <= 2n");
            BOOST_MATH_ASSERT_MSG(approximation_order >= 2,
                             "The approximation order must be >= 2");

            if constexpr (std::is_same_v<Real, float> || std::is_same_v<Real, double>)
            {
                auto interior = detail::interior_velocity_filter<long double>(n, approximation_order);
                m_f.resize(interior.size());
                for (std::size_t j = 0; j < interior.size(); ++j)
                {
                    m_f[j] = static_cast<Real>(interior[j])/m_dt;
                }
            }
            else
            {
                m_f = detail::interior_velocity_filter<Real>(n, approximation_order);
                for (auto & x : m_f)
                {
                    x /= m_dt;
                }
            }

            m_boundary_filters.resize(n);
            // This for loop is a natural candidate for parallelization.
            // But does it matter? Probably not.
            for (std::size_t i = 0; i < n; ++i)
            {
                if constexpr (std::is_same_v<Real, float> || std::is_same_v<Real, double>)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    auto bf = detail::boundary_velocity_filter<long double>(n, approximation_order, s);
                    m_boundary_filters[i].resize(bf.size());
                    for (std::size_t j = 0; j < bf.size(); ++j)
                    {
                        m_boundary_filters[i][j] = static_cast<Real>(bf[j])/m_dt;
                    }
                }
                else
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    m_boundary_filters[i] = detail::boundary_velocity_filter<Real>(n, approximation_order, s);
                    for (auto & bf : m_boundary_filters[i])
                    {
                        bf /= m_dt;
                    }
                }
            }
        }
        else if constexpr (order == 2)
        {
            // High precision isn't warranted for small p; only for large p.
            // (The computation appears stable for large n.)
            // But given that the filters are reusable for many vectors,
            // it's better to do a high precision computation and then cast back,
            // since the resulting cost is a factor of 2, and the cost of the filters not working is hours of debugging.
            if constexpr (std::is_same_v<Real, double> || std::is_same_v<Real, float>)
            {
                auto f = detail::acceleration_filter<long double>(n, approximation_order, 0);
                m_f.resize(n+1);
                for (std::size_t i = 0; i < m_f.size(); ++i)
                {
                    m_f[i] = static_cast<Real>(f[i+n])/(m_dt*m_dt);
                }
                m_boundary_filters.resize(n);
                for (std::size_t i = 0; i < n; ++i)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    auto bf = detail::acceleration_filter<long double>(n, approximation_order, s);
                    m_boundary_filters[i].resize(bf.size());
                    for (std::size_t j = 0; j < bf.size(); ++j)
                    {
                        m_boundary_filters[i][j] = static_cast<Real>(bf[j])/(m_dt*m_dt);
                    }
                }
            }
            else
            {
                // Given that the purpose is denoising, for higher precision calculations,
                // the default precision should be fine.
                auto f = detail::acceleration_filter<Real>(n, approximation_order, 0);
                m_f.resize(n+1);
                for (std::size_t i = 0; i < m_f.size(); ++i)
                {
                    m_f[i] = f[i+n]/(m_dt*m_dt);
                }
                m_boundary_filters.resize(n);
                for (std::size_t i = 0; i < n; ++i)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    m_boundary_filters[i] = detail::acceleration_filter<Real>(n, approximation_order, s);
                    for (auto & bf : m_boundary_filters[i])
                    {
                        bf /= (m_dt*m_dt);
                    }
                }
            }
        }
        else
        {
            BOOST_MATH_ASSERT_MSG(false, "Derivatives of order 3 and higher are not implemented.");
        }
    }

    Real get_spacing() const
    {
        return m_dt;
    }

    template<class RandomAccessContainer>
    Real operator()(RandomAccessContainer const & v, std::size_t i) const
    {
        static_assert(std::is_same_v<typename RandomAccessContainer::value_type, Real>,
                      "The type of the values in the vector provided does not match the type in the filters.");

        BOOST_MATH_ASSERT_MSG(std::size(v) >= m_boundary_filters[0].size(),
            "Vector must be at least as long as the filter length");

        if constexpr (order==1)
        {
            if (i >= m_f.size() - 1 && i <= std::size(v) - m_f.size())
            {
                // The filter has length >= 1:
                Real dvdt = m_f[1] * (v[i + 1] - v[i - 1]);
                for (std::size_t j = 2; j < m_f.size(); ++j)
                {
                    dvdt += m_f[j] * (v[i + j] - v[i - j]);
                }
                return dvdt;
            }

            // m_f.size() = N+1
            if (i < m_f.size() - 1)
            {
                auto &bf = m_boundary_filters[i];
                Real dvdt = bf[0]*v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[j];
                }
                return dvdt;
            }

            if (i > std::size(v) - m_f.size() && i < std::size(v))
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real dvdt = bf[0]*v[std::size(v)-1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[std::size(v) - 1 - j];
                }
                return -dvdt;
            }
        }
        else if constexpr (order==2)
        {
            if (i >= m_f.size() - 1 && i <= std::size(v) - m_f.size())
            {
                Real d2vdt2 = m_f[0]*v[i];
                for (std::size_t j = 1; j < m_f.size(); ++j)
                {
                    d2vdt2 += m_f[j] * (v[i + j] + v[i - j]);
                }
                return d2vdt2;
            }

            // m_f.size() = N+1
            if (i < m_f.size() - 1)
            {
                auto &bf = m_boundary_filters[i];
                Real d2vdt2 = bf[0]*v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[j];
                }
                return d2vdt2;
            }

            if (i > std::size(v) - m_f.size() && i < std::size(v))
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real d2vdt2 = bf[0] * v[std::size(v) - 1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[std::size(v) - 1 - j];
                }
                return d2vdt2;
            }
        }

        // OOB access:
        std::string msg = "Out of bounds access in Lanczos derivative.";
        msg += "Input vector has length " + std::to_string(std::size(v)) + ", but user requested access at index " + std::to_string(i) + ".";
        throw std::out_of_range(msg);
        return std::numeric_limits<Real>::quiet_NaN();
    }

    template<class RandomAccessContainer>
    void operator()(RandomAccessContainer const & v, RandomAccessContainer & w) const
    {
        static_assert(std::is_same_v<typename RandomAccessContainer::value_type, Real>,
                      "The type of the values in the vector provided does not match the type in the filters.");
        if (&w[0] == &v[0])
        {
            throw std::logic_error("This transform cannot be performed in-place.");
        }

        if (std::size(v) < m_boundary_filters[0].size())
        {
            std::string msg = "The input vector must be at least as long as the filter length. ";
            msg += "The input vector has length = " + std::to_string(std::size(v)) + ", the filter has length " + std::to_string(m_boundary_filters[0].size());
            throw std::length_error(msg);
        }

        if (std::size(w) < std::size(v))
        {
            std::string msg = "The output vector (containing the derivative) must be at least as long as the input vector.";
            msg += "The output vector has length = " + std::to_string(std::size(w)) + ", the input vector has length " + std::to_string(std::size(v));
            throw std::length_error(msg);
        }

        if constexpr (order==1)
        {
            for (std::size_t i = 0; i < m_f.size() - 1; ++i)
            {
                auto &bf = m_boundary_filters[i];
                Real dvdt = bf[0] * v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[j];
                }
                w[i] = dvdt;
            }

            for(std::size_t i = m_f.size() - 1; i <= std::size(v) - m_f.size(); ++i)
            {
                Real dvdt = m_f[1] * (v[i + 1] - v[i - 1]);
                for (std::size_t j = 2; j < m_f.size(); ++j)
                {
                    dvdt += m_f[j] *(v[i + j] - v[i - j]);
                }
                w[i] = dvdt;
            }

            for(std::size_t i = std::size(v) - m_f.size() + 1; i < std::size(v); ++i)
            {
                int k = std::size(v) - 1 - i;
                auto &f = m_boundary_filters[k];
                Real dvdt = f[0] * v[std::size(v) - 1];;
                for (std::size_t j = 1; j < f.size(); ++j)
                {
                    dvdt += f[j] * v[std::size(v) - 1 - j];
                }
                w[i] = -dvdt;
            }
        }
        else if constexpr (order==2)
        {
            // m_f.size() = N+1
            for (std::size_t i = 0; i < m_f.size() - 1; ++i)
            {
                auto &bf = m_boundary_filters[i];
                Real d2vdt2 = 0;
                for (std::size_t j = 0; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[j];
                }
                w[i] = d2vdt2;
            }

            for (std::size_t i = m_f.size() - 1; i <= std::size(v) - m_f.size(); ++i)
            {
                Real d2vdt2 = m_f[0]*v[i];
                for (std::size_t j = 1; j < m_f.size(); ++j)
                {
                    d2vdt2 += m_f[j] * (v[i + j] + v[i - j]);
                }
                w[i] = d2vdt2;
            }

            for (std::size_t i = std::size(v) - m_f.size() + 1; i < std::size(v); ++i)
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real d2vdt2 = bf[0] * v[std::size(v) - 1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[std::size(v) - 1 - j];
                }
                w[i] = d2vdt2;
            }
        }
    }

    template<class RandomAccessContainer>
    RandomAccessContainer operator()(RandomAccessContainer const & v) const
    {
        RandomAccessContainer w(std::size(v));
        this->operator()(v, w);
        return w;
    }

    // Don't copy; too big.
    discrete_lanczos_derivative( const discrete_lanczos_derivative & ) = delete;
    discrete_lanczos_derivative& operator=(const discrete_lanczos_derivative&) = delete;

    // Allow moves:
    discrete_lanczos_derivative(discrete_lanczos_derivative&&) = default;
    discrete_lanczos_derivative& operator=(discrete_lanczos_derivative&&) = default;

private:
    std::vector<Real> m_f;
    std::vector<std::vector<Real>> m_boundary_filters;
    Real m_dt;
};

} // namespaces
#endif


//
// Created by Anshuman Mishra on 11/26/19.
//

#include "headers/Policy.h"
#include <algorithm>
#include <random>
#include <functional>
#include <boost/math/special_functions/beta.hpp>
#include "headers/MathUtils.h"

using namespace std;
using namespace Eigen;

Policy::Policy (int numActions_, int stateTerms_, unsigned seed){
//  cout << "Initializing without theta" << endl;
//  cout << "Theta before " << theta.size() << endl;
  // init theta
  numActions = numActions_;
  stateTerms = stateTerms_;
  theta = MatrixXd(stateTerms, numActions);
  //  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  gen = default_random_engine (seed);
//  cout << "Theta after " << theta.size() << endl;
}

Policy::Policy (VectorXd& theta_init, int numActions_, int stateTerms_, unsigned seed){
  numActions = numActions_;
  stateTerms = stateTerms_;
//  cout << "Initializing with theta " << theta_init.size() << endl;
//  cout << "Theta before " << theta.size() << endl;
  theta = MatrixXd::Map(theta_init.data(),  stateTerms, numActions);
//  cout << "Theta after " << theta.size() << endl;
  gen = default_random_engine (seed);
}

Policy::Policy(Policy pi, unsigned seed){
  numActions = pi.getNumActions();
  stateTerms = pi.getStateTerms();
  theta = MatrixXd::Map(pi.getTheta().data(), stateTerms, numActions);
  gen = default_random_engine (seed);
}

void Policy::setTheta(const VectorXd& newTheta) {
//  cout << "Setting theta " << newTheta.size() << endl;
//  cout << "Theta before " << theta.size() << endl;
  theta = MatrixXd::Map(newTheta.data(), theta.rows(), theta.cols());
//  cout << "Theta after " << theta.size() << endl;
}

VectorXd Policy::getTheta() const{
  return VectorXd::Map(theta.data(), theta.size());
}

int Policy::getNumActions() const{
  return numActions;
}

int Policy::getStateTerms() const{
  return stateTerms;
}

// Softmax Action Selection with Linear Function Approximation
int Policy::getAction (const VectorXd& phi){
//  cout << "Getting Action" << endl;
//  cout << phi.cols() << "x" << phi.rows() << "dot" << theta.rows() << "x" << theta.cols()  << endl;
//  cout << "Phi " << phi.transpose() << endl;
//  cout << "Theta\n " << theta << endl;
  VectorXd dot = phi.transpose()*theta;
  VectorXd q = exp(dot.array() - dot.maxCoeff());
//  cout << "q " << q << endl;

  discrete_distribution<int> dist(q.data(), q.data() + q.rows() * q.cols());
  int action = dist(gen);
//  cout << "Action:" << action << endl;
  return action;
}

double Policy::getActionProbability (const VectorXd& phi, int action) const {
//  cout << "Getting Action Probability";
//  cout << phi.cols() << "x" << phi.rows() << "dot" << theta.rows() << "x" << theta.cols()  << endl;
//  cout << "Phi " << phi.transpose() << endl;
//  cout << "Theta\n" << theta << endl;
  VectorXd dot = phi.transpose()*theta;
//  cout << "dot " << dot.transpose() << endl;
  VectorXd q = exp(dot.array() - dot.maxCoeff());

  discrete_distribution<int> dist(q.data(), q.data() + q.rows() * q.cols());
  double p = dist.probabilities()[action];
  if (isnan(p))
    p = 1.0;
//  if (isnan(p)){
//    cout << "Action Probability is nan" << endl;
//    cout << "phi: " << phi.transpose() << endl;
//    cout << "Q: " << q.transpose() << endl;
//    cout << "action: " << action << endl;
//    getchar();
//  }
//  cout << "p=" << p << endl;
  return p;
}


//==================================================================================================
/*!
  @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_ILOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ILOG2_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/clz.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(ilog2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return bs::exponent(a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(ilog2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return saturated_(dec)(sizeof(bd::scalar_of_t<A0>)*8-bs::clz(bitwise_cast<result>(a0)));
      }
   };

} } }

#endif


#ifdef MEX

#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#endif
#include <igl/matlab/MexStream.h>

#include <igl/per_vertex_normals.h>
#include <igl/parallel_for.h>
#include <igl/signed_distance.h>
#include <igl/per_edge_normals.h>
#include <igl/matlab/validate_arg.h>
#include <igl/per_face_normals.h>
#include <igl/WindingNumberAABB.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/C_STR.h>

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

void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & P,
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  igl::SignedDistanceType & type)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const int dim = mxGetN(prhs[0]);

  parse_rhs_double(prhs,P);
  parse_rhs_double(prhs+1,V);
  parse_rhs_index(prhs+2,F);

  mexErrMsgTxt(P.cols()==3 || P.cols()==2,"P must be #P by (3|2)");
  mexErrMsgTxt(V.cols()==3 || V.cols()==2,"V must be #V by (3|2)");
  mexErrMsgTxt(V.cols()==P.cols(),"dim(V) must be dim(P)");
  mexErrMsgTxt(F.cols()==V.cols(),"F must be #F by dim(V)");

  type = SIGNED_DISTANCE_TYPE_PSEUDONORMAL;
  {
    int i = 3;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("SignedDistanceType",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * type_name = mxArrayToString(prhs[++i]);
        if(strcmp("pseudonormal",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_PSEUDONORMAL;
        }else if(strcmp("winding_number",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_WINDING_NUMBER;
        }else
        {
          mexErrMsgTxt(false,C_STR("Unknown SignedDistanceType: "<<type_name));
        }
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }
}

//void precompute(
//  const Eigen::MatrixXd & V,
//  const Eigen::MatrixXi & F,
//  const igl::SignedDistanceType sign_type,
//  State & state)
//{
//  using namespace igl;
//  using namespace std;
//  using namespace Eigen;

  // This will remember the data structures for subsequent calls (without an
  // calls in between with different (V,F) or type
  static Eigen::MatrixXd g_V;
  static Eigen::MatrixXi g_F;
  static igl::SignedDistanceType g_sign_type =
    igl::NUM_SIGNED_DISTANCE_TYPE;
  static igl::AABB<Eigen::MatrixXd,3> g_tree;
  static igl::WindingNumberAABB<
    Eigen::RowVector3d,
    Eigen::MatrixXd,
    Eigen::MatrixXi> g_hier;
  static Eigen::MatrixXd g_FN,g_VN,g_EN;
  static Eigen::MatrixXi g_E;
  static Eigen::VectorXi g_EMAP;

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd P,V,C,N;
  MatrixXi F;
  VectorXi I;
  VectorXd S;
  SignedDistanceType type;
  parse_rhs(nrhs,prhs,P,V,F,type);

  if(F.rows() > 0)
  {
    switch(V.cols())
    {
      case 2:
      {
        // Persistent data not supported for 2D
        signed_distance(P,V,F,type,S,I,C,N);
        break;
      }
      case 3:
      {
        if(g_sign_type != type || g_V != V || g_F != F)
        {
          g_V = V;
          g_F = F;
          g_sign_type = type;
          // Clear the tree
          g_tree.deinit();

          // Prepare distance computation
          g_tree.init(V,F);
          switch(type)
          {
            default:
              assert(false && "Unknown SignedDistanceType");
            case SIGNED_DISTANCE_TYPE_DEFAULT:
            case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
              g_hier.set_mesh(V,F);
              g_hier.grow();
              break;
            case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
              // "Signed Distance Computation Using the Angle Weighted Pseudonormal"
              // [Bærentzen & Aanæs 2005]
              per_face_normals(V,F,g_FN);
              per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,
                g_FN,g_VN);
              per_edge_normals(
                V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,
                g_FN,g_EN,g_E,g_EMAP);
              break;
          }
        }

        N.resize(P.rows(),3);
        S.resize(P.rows(),1);
        I.resize(P.rows(),1);
        C.resize(P.rows(),3);
        //for(int p = 0;p<P.rows();p++)
        igl::parallel_for(P.rows(),[&](const int p)
        {
          const Eigen::RowVector3d q(P(p,0),P(p,1),P(p,2));
          double s,sqrd;
          Eigen::RowVector3d c;
          int i;
          switch(type)
          {
            default:
              assert(false && "Unknown SignedDistanceType");
            case SIGNED_DISTANCE_TYPE_DEFAULT:
            case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
              signed_distance_winding_number(
                g_tree,g_V,g_F,g_hier,q,s,sqrd,i,c);
              break;
            case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
            {
              RowVector3d n(0,0,0);
              signed_distance_pseudonormal(
                g_tree,g_V,g_F,g_FN,g_VN,g_EN,g_EMAP,
                q,s,sqrd,i,c,n);
              N.row(p) = n;
              break;
            }
          }
          I(p) = i;
          S(p) = s*sqrt(sqrd);
          C.row(p) = c;
        },10000);
        break;
      }
    }
  }

  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 4:
    {
      prepare_lhs_double(N,plhs+3);
      // Fall through
    }
    case 3:
    {
      prepare_lhs_double(C,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(I,plhs+1);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_double(S,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}


/**
 * Conversion routines. Convert from coordinate type to coordinate type
 * via from_type foo = converter<to_type>()(from_type bar)
 *
 * Copyright 2013 Bruce Ide
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 */

#include "coordinates.hpp"
#include <Eigen/Core>
#include <type_traits>

#ifndef _HPP_CONVERTS
#define _HPP_CONVERTS

namespace fr {

  namespace coordinates {

    template <typename convert_to>
    struct converter {

    };

    /***************************************************************
     * Put convert to lat_long bits here
     */

    template <>
    struct converter<lat_long> {

      // Converting from lat_long to lat_long is kind of odd, but I'll
      // cover the case anyway
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,lat_long>::value,lat_long>::type
	operator()(const convert_from &c)
      {
	return c;
      }

      // ECEF to Latlong
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,lat_long>::type
      operator()(const convert_from &xyz, const ellipsoid_parameters &e = WGS84_ELLIPSOID, double tolerance = 0.0000000001 )
      {
	double diff = 2 * tolerance;
	double t = e.ee * xyz.get_z();
	double n = 0.0;
	double nph = 0.0;
	double sinPhi = 0.0;
	double lat;
	double longitude;
	double alt;

	longitude = atan2(xyz.get_y(), xyz.get_x()) * 180 / fr::constants::pi;
	while(diff > tolerance) {
	  double zT = xyz.get_z() + t;
	  nph = sqrt(pow(xyz.get_x(), 2) + pow(xyz.get_y(), 2) + pow(zT, 2));
	  sinPhi = zT / nph;
	  n = e.ae / sqrt(1 - e.ee * sinPhi * sinPhi);
	  double told = t;
	  t = n * e.ee * sinPhi;
	  diff = fabs(t - told);
	}

	lat = asin(sinPhi) * 180 / fr::constants::pi;
	alt = nph - n;
	lat_long retval(lat, longitude, alt);
	return retval;
      }

      // ecef_vel to lat_long (Loses velocity information)

      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,lat_long>::type
      operator()(const convert_from &c, const ellipsoid_parameters &e = WGS84_ELLIPSOID)
      {
	ecef interim = converter<ecef>()(c);
	lat_long retval = converter<lat_long>()(interim, e);
	return retval;
      }

      // tod_eci_vel to latlong (Loses velocity information)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,lat_long>::type
      operator()(const convert_from &c, const double &t, const ellipsoid_parameters e = WGS84_ELLIPSOID)
      {
	// Convert from tod_eci to ecef_vel
	ecef_vel interim = converter<ecef_vel>()(c,t);
	// Then use the function before this one to convert to lat/long
	lat_long retval = converter<lat_long>()(interim, e);
	return retval;
      }

    };

    /*********************************************************
     * Put convert to ECEF bits here
     */

    template <>
    struct converter<ecef> {

      // ECEF to ECEF conversion.
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,ecef>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // latlong to ecef
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,lat_long>::value,ecef>::type
      operator()(const convert_from &c, const ellipsoid_parameters &e = WGS84_ELLIPSOID)
      {
	double x,y,z;
	const double &pi = fr::constants::pi;
	double slat = sin(c.get_lat() * pi / 180);
	double clat = cos(c.get_lat() * pi / 180);
	double slon = sin(c.get_long() * pi / 180);
	double clon = cos(c.get_long() * pi / 180);
	double n = e.ae / sqrt(1.0 - e.ee * pow(slat, 2));
	x = (n + c.get_alt()) * clat * clon;
	y = (n + c.get_alt()) * clat * slon;
	z = (n * (1.0 - e.ee) + c.get_alt()) * slat;
	ecef retval(x,y,z);
	return retval;
      }

      // ecef_vel to ecef (loses velocity information)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,ecef>::type
      operator()(const convert_from &c)
      {
	ecef retval(c.get_x(), c.get_y(), c.get_z());
	return retval;
      }

      // tod_eci to ecef (Requires time coordinate was measured)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci>::value,ecef>::type
      operator()(const convert_from &c, const double &at_time)
      {
	eci_to_ecef conversion_matrix(at_time);
	Eigen::Vector3d c_vec = c.get_xyz();
	Eigen::Matrix3d c_mat = conversion_matrix.get();
	Eigen::Vector3d interim = c_mat * c_vec;
	ecef retval(interim(0), interim(1), interim(2));
	return retval;
      }

    };

    /**********************************************************
     * Put tod_eci stuff here
     */

    template<>
    struct converter<tod_eci> {

      // ECI to ECI conversion

      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci>::value,tod_eci>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // ecef to tod_eci (Requires time coordinate was measured)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,tod_eci>::type
      operator()(const convert_from &c, const double &time_at)
      {
	ecef_to_eci conversion_matrix(time_at);
	Eigen::Vector3d c_vec = c.get_xyz();
	Eigen::Matrix3d c_mat = conversion_matrix.get();
	Eigen::Vector3d interim = c_mat * c_vec;
	tod_eci retval(interim(0), interim(1), interim(2));
	return retval;
      }

    };

    /***************************************************************
     * Put ecef_vel stuff here
     */

    template<>
    struct converter<ecef_vel> {
      // ecef_vel to ecef_vel conversion
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,ecef_vel>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // tod_eci_vel to ecef_vel (requires time component)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,ecef_vel>::type
      operator()(const convert_from &c, const double &time_at)
      {
	Eigen::Matrix<double,6,1> vec = c.get_vector();
	eci_to_ecef cm(time_at);
	Eigen::Matrix<double,6,6> c_mat = cm.get_xyz_vel();
	Eigen::Matrix<double,6,1> interim = c_mat * vec;
	ecef_vel retval(interim(0), interim(1), interim(2), interim(3), interim(4), interim(5));
	return retval;
      }

    };

    /********************************************************************
     * Put tod_eci_vel stuff here
     */

    template<>
    struct converter<tod_eci_vel> {

      // From tod_eci_vel to tod_eci_vel
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,tod_eci_vel>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // from ecef_vel to tod_eci_vel
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,tod_eci_vel>::type
      operator()(const convert_from &c, const double &t)
      {
	ecef_to_eci cm(t);
	Eigen::Matrix<double,6,1> vec = c.get_vector();
	Eigen::Matrix<double,6,6> c_mat = cm.get_xyz_vel();
	Eigen::Matrix<double,6,1> interim = c_mat * vec;
	tod_eci_vel retval(interim(0), interim(1), interim(2), interim(3), interim(4), interim(5));
	return retval;
      }

    };

  }

}

#endif


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

#include "rlbot/bot.h"
#include "rlbot/rlbot_generated.h"
#include "rlbot/scopedrenderer.h"

#include "util.h"

using namespace Eigen;

namespace util {

	const float M_PI = 3.14159265;

	Vector3f convert(const rlbot::flat::Vector3& vec) {
		return { vec.x(), vec.y(), vec.z() };
	}

	Vector3f convert(const rlbot::flat::Rotator& vec) {

		return { vec.pitch(), vec.yaw(), vec.roll() };
	}

	rlbot::flat::Vector3 convert(const Vector3f& vec) {
		return rlbot::flat::Vector3{ vec[0], vec[1], vec[2] };
	}

	Physics::Physics(const rlbot::flat::Physics& phys) :
		location(convert(*phys.location())),
		velocity(convert(*phys.velocity())),
		rotation(convert(*phys.rotation())),
		angularVelocity(convert(*phys.angularVelocity())) {}

	Physics::Physics(const BallPhysics& bp) :
		location(bp.location),
		velocity(bp.velocity),
		rotation(bp.rotation),
		angularVelocity(bp.angularVelocity) {}

	BallPhysics::BallPhysics(const rlbot::flat::Physics& phys) :
		location(convert(*phys.location())),
		velocity(convert(*phys.velocity())),
		rotation(Vector3f::Zero()),
		angularVelocity(convert(*phys.angularVelocity())) {}

	Vector3f Physics::forward() const {

		float	sp = sin(rotation.pitch),
				cp = cos(rotation.pitch),
				sr = sin(rotation.roll),
				cr = cos(rotation.roll),
				sy = sin(rotation.yaw),
				cy = cos(rotation.yaw);

		return { cp * cy, cp * sy, sp };
	}

	Vector3f Physics::right() const {
		//pitch roll yaw
		float	sp = sin(rotation.pitch),
				cp = cos(rotation.pitch),
				sr = sin(rotation.roll),
				cr = cos(rotation.roll),
				sy = sin(rotation.yaw),
				cy = cos(rotation.yaw);

		return { cy * sp * sr - cr * sy, sy * sp * sr + cr * cy, -cp * sr };

	}

	Vector3f Physics::up() const {

		float	sp = sin(rotation.pitch),
				cp = cos(rotation.pitch),
				sr = sin(rotation.roll),
				cr = cos(rotation.roll),
				sy = sin(rotation.yaw),
				cy = cos(rotation.yaw);

		return { -cr * cy * sp - sr * sy, -cr * sy * sp + sr * cy, cp * cr };
	}

	Rotation::operator Vector3f() const {
		return Vector3f(pitch, yaw, roll);
	}

	Rotation::Rotation(const Vector3f& rot) :
		pitch(rot[0]),
		yaw(rot[1]),
		roll(rot[2]) {
	}

	Rotation::Rotation(const rlbot::flat::Rotator& rot) :
		pitch(rot.pitch()),
		yaw(rot.yaw()),
		roll(rot.roll()) {}

	ScoreInfo::ScoreInfo(const rlbot::flat::ScoreInfo& inf) :
		score(inf.score()),
		goals(inf.goals()),
		ownGoals(inf.ownGoals()),
		assists(inf.assists()),
		saves(inf.saves()),
		shots(inf.shots()),
		demolitions(inf.demolitions()) {}

	BoxShape::BoxShape(const rlbot::flat::BoxShape& box) :
		length(box.length()),
		width(box.width()),
		height(box.height()) {}

	Car::Car(const rlbot::flat::PlayerInfo& info) :
		physics(*info.physics()),
		scoreInfo(*info.scoreInfo()),
		isDemolished(info.isDemolished()),
		hasWheelContact(info.hasWheelContact()),
		isSupersonic(info.isSupersonic()),
		isBot(info.isBot()),
		jumped(info.jumped()),
		doubleJumped(info.doubleJumped()),
		name((*info.name()).str()),
		team(info.team()),
		boost(info.boost()),
		hitbox(*info.hitbox()),
		hitboxOffset(convert(*info.hitboxOffset())) {
	}

	Touch::Touch(const rlbot::flat::Touch& t) :
		gameSeconds(t.gameSeconds()),
		location(convert(*t.location())),
		normal(convert(*t.normal())),
		team(t.team()),
		playerIndex(t.playerIndex()) {}

	DropShotBallInfo::DropShotBallInfo(const rlbot::flat::DropShotBallInfo& drop) :
		absorbedForce(drop.absorbedForce()),
		damageIndex(drop.damageIndex()),
		forceAccumRecent(drop.forceAccumRecent()) {}

	CollisionShape::CollisionShape(rlbot::flat::CollisionShape coll) : type(0) {} //TODO FIX TYPE

	Ball::Ball(const rlbot::flat::BallInfo& ball) :
		physics(*ball.physics()),
		latestTouch(*ball.latestTouch()),
		dropShotInfo(*ball.dropShotInfo()),
		shape_type(ball.shape_type()) {}

	PredictionSlice::PredictionSlice(const rlbot::flat::PredictionSlice* slice) :
		gameSeconds(slice->gameSeconds()),
		physics(*slice->physics()) {}

	BallPrediction::BallPrediction(const rlbot::flat::BallPrediction& bp){
		int max = bp.slices()->size();
		for (int i = 0; i < max; ++i) {
			predictionSlices.push_back(bp.slices()->Get(i));

		}
	}

	Vector3f Car::other_goal() const{
		float t = team == 1 ? 1 : -1;
		return Vector3f{ 0, -5120 * t, 92.75 };
	}

	Vector3f Car::own_goal() const{
		float t = team == 1 ? 1 : -1;
		return Vector3f{ 0,  5120 * t, 92.75 };
	}

	Vector3f to_local(Car c, Vector3f target) {

		//TODO MAKE ACTUAL MATRIX IMPLENTATION INSTEAD OF WHATEVER THIS IS

		Vector3f local = {
			(target - c.physics.location).dot(c.physics.forward()),
			(target - c.physics.location).dot(c.physics.right()),
			(target - c.physics.location).dot(c.physics.up())
		};

		return local;
	}

	template<typename T>
	T map(T value, T old_min, T old_max, T new_min, T new_max) {
		return (value - old_min) * (new_max - new_min) / (old_max - old_min) + new_min;
	}

	template<typename T>
	T clamp(T value, T min, T max) {
		if (value < min) return min;
		if (value > max) return max;
		return value;
	}

	rlbot::Controller optimalGroundControl(const Car& car, const std::vector<Vector3f>& path) {

		bool debug = 1;

		rlbot::Controller controller{ 0 };

		//take average of first 20% of the path (10)
		Vector3f aim = { 0, 0, 0 };
		int len = std::min((int)path.size(), util::NUM_POINTS/5);
		for (auto i = 0; i < len; ++i) aim += path[i];
		aim /= len;

		//aim = path[path.size() / 2];

		//aim = path[0];

		rlbot::ScopedRenderer renderer("OPTIMAL GROUND CONTROL " + std::to_string(car.team));

		//Vector3f local = to_local(car, aim);

		float angle = atan2(aim.y() - car.physics.location.y(),
							aim.x() - car.physics.location.x());

		angle -= car.physics.rotation.yaw;

		if (angle < -M_PI) angle += 2 * M_PI;
		if (angle > M_PI) angle -= 2 * M_PI;

		float steer = angle/M_PI;

		steer *= 10;
		steer = clamp<float>(steer, -1, 1);

		//steer = steer > 0 ? 1 : -1;
		//steer = clamp<float>(steer, -1, 1);

		if (debug && car.team == 0) {
			renderer.DrawRect3D(rlbot::Color::black, convert(aim),
				20, 20, true, true);
		}

		//if steer is way too big, slow down

		//bool is_going_forward = ( car.physics.forward().dot(car.physics.velocity) > 0);
		/*
		if (is_going_forward && path[path.size() -1].z() > 400) {
			controller.throttle = 1 - 2 * std::abs(steer);
		}
		else {
			controller.throttle = 1;
		}*/

		controller.throttle = clamp<float>(1.3 - std::abs(steer), 0, 1);

		if (steer > 0.05) steer = 1;
		if (steer < -0.05) steer = -1;

		controller.steer = steer;

		if (std::abs(steer) < 0.05 && car.hasWheelContact) controller.boost = 1;

		//if car is stuck

		Vector3f origin = { 0, 0, 92.75 };

		// If ball is at center(kickoff)
		if ((path[path.size() - 1] - origin).norm() < 5) {
			controller.boost = 1;
			controller.steer = controller.steer > 0 ? 1 : -1;
		}//else if (car.physics.velocity.norm() < 5) controller.jump = 1;

		//jump if car is close to ball
		if ((path[path.size() - 1] - path[0]).norm() < 200 && car.hasWheelContact && path[path.size() - 1].z() < 200 && std::abs(controller.steer) < 0.1 ) {
			controller.jump = 1;
		}

		// If car is in the air
		if (!car.hasWheelContact) {
			if ((path[path.size() - 1] - path[0]).norm() < 200) {
				controller.pitch = 1;

				controller.jump = 1;
			}
			else {
				controller.pitch = clamp<float>(-2*car.physics.rotation.pitch, -1, 1);
				controller.yaw = clamp<float>(2*controller.steer, -1, 1);
				controller.roll = clamp<float>(-2*car.physics.rotation.roll, -1, 1);
			}
		}

		//if target is in the air slow down
		Vector3f target = path[path.size() - 1];
		target.z() = 0;

		Vector3f now = car.physics.location;
		now.z() = 0;

		float groundDist = (target - now).norm();

		//if (path[path.size() - 1].z() > 300 && groundDist < 500 && std::abs(controller.steer) < 0.1)
			//controller.throttle = -1 * is_going_forward;

		if (debug && car.team == 0) {
			renderer.DrawString2D(	"boost: " + std::to_string(controller.boost) + "\n" +
									"handbrake: " + std::to_string(controller.handbrake) + "\n" +
									"jump: " + std::to_string(controller.jump) + "\n" +
									"steer: " + std::to_string(controller.steer) + "\n" +
									"throttle: " + std::to_string(controller.throttle) + "\n" +
									"pitch: " + std::to_string(controller.pitch) + "\n" +
									"roll: " + std::to_string(controller.roll) + "\n" +
									"yaw: " + std::to_string(controller.yaw) + "\n",
									rlbot::Color::green,
									rlbot::flat::Vector3{ 10, 200, 0 },
									2, 2);

		}

		return controller;

	}

}//namespace util


#include <cmath>
#include <fstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <complex>
#include <tuple>
#include <algorithm>
#include <boost/range/iterator_range.hpp>

#include "common.hpp"
#include "fft.hpp"
#include "spectrum_image.hpp"
#include "segment_envelope.hpp"

spectrum_image::spectrum_image(
  const window_list_t &window,
  const std::vector< int16_t > &audio,
  int x_,
  uint32_t sample_rate_,
  uint32_t resolution_,
  uint32_t scale_,
  int weight,
  unsigned int interval
) : x( x_ ), resolution( resolution_ ), scale( scale_ ), sample_rate( sample_rate_ ) {
  a = powf( 2.f, float( scale ) + weight );
  b = float( interval ) * float( scale );
  const auto converted = fftref( window, audio, resolution, a, b, x );
  pixels_begin = converted.second.get();
  pixels = converted.second;
  envelope = std::move( converted.first );
  const auto delay_end = std::find_if( envelope.begin(), envelope.end(), []( float v ) { return v != 0.f; } );
  const auto delay_size = std::distance( envelope.begin(), delay_end );
  pixels_begin += delay_size * x;
  envelope.erase( envelope.begin(), delay_end );
  y = envelope.size();
  const auto tail_blank_end = std::find_if( envelope.rbegin(), envelope.rend(), []( float v ) { return v != 0.f; } );
  const auto tail_blank_size = std::distance( envelope.rbegin(), tail_blank_end );
  envelope.resize( envelope.size() - tail_blank_size );
  y = envelope.size();
  std::tie( delay, attack, release ) = segment_envelope( envelope, a, b );
  delay_time = ( a * delay * delay + b * delay ) * tinyfm3::delta;
  attack_time = ( a * attack * attack + b * attack ) * tinyfm3::delta;
  release_time = ( a * release * release + b * release ) * tinyfm3::delta;
  total_time = ( a * envelope.size() * envelope.size() + b * envelope.size() ) * tinyfm3::delta;
  std::cout << __FILE__ << " " << __LINE__ << " " << delay_time << " " << attack_time << " " << release_time << " " << total_time << std::endl;
}
float get_distance(
  const spectrum_image &ref,
  const window_list_t &window,
  const std::vector< int16_t > &audio
) {
  const float a = ref.get_a();
  const float b = ref.get_b();
  const auto converted = fftcomp( ref.get_pixels(), ref.get_height(), window, audio, ref.get_resolution(), a, b, ref.get_width() );
  float delay, attack, release;
  std::tie( delay, attack, release ) = segment_envelope( converted.second, ref.get_a(), ref.get_b() );
  double delay_time = ( a * delay * delay + b * delay ) * tinyfm3::delta;
  double attack_time = ( a * attack * attack + b * attack ) * tinyfm3::delta;
  double release_time = ( a * release * release + b * release ) * tinyfm3::delta;
  double delay_distance = std::abs( ref.get_delay_time() - delay_time );
  double attack_distance = std::abs( ref.get_attack_time() - attack_time );
  double release_distance = std::abs( ref.get_release_time() - release_time );
//  std::cout << delay_distance << " " << attack_distance << " " << release_distance << std::endl;
  return double( converted.first )/ref.get_width()/ref.get_height() * ( delay_distance * 40.f + attack_distance * 40.f + release_distance * 40.f + 1.f );
}


#ifndef CHARGE_EV_PHEM_HPP
#define CHARGE_EV_PHEM_HPP

#include "common/constant_function.hpp"
#include "common/serialization.hpp"

#include "ev/consumption_model.hpp"

#include <boost/algorithm/string.hpp>

#include <cstdint>

namespace charge::ev {
namespace detail {

enum class PHEMVehicleType : std::int32_t {
    PEUGEOT_ION,
    EV_NO_AUX,
    EV_SPRING,
    EV_SUMMER,
    EV_WINTER,
    INVALID = -1
};

class PHEMConsumptionModel : public ConsumptionModel {
  public:
    PHEMConsumptionModel(double speed_parameter, double slope_parameter, double const_parameter)
        : speed_parameter(speed_parameter), slope_parameter(slope_parameter),
          const_parameter(const_parameter) {}

    LimitedTradeoffFunction tradeoff_function(const double length, const double slope,
                                              const double min_speed,
                                              const double max_speed) const override final {
        assert(min_speed <= max_speed);
        assert(min_speed > 0);
        assert(max_speed < 300);

        const double limited_slope = std::max(-10.0, slope*100);
        // Original code has a scaling by 1000, this causes numeric problems.
        // const double a = 1000.0 * length * speed_parameter * (3.6 * 3.6 * length * length);
        // const double b = 0.0;
        // const double c = 1000.0 * length * (slope_parameter * limited_slope + const_parameter);
        const double a = length * speed_parameter * (3.6 * 3.6 * length * length);
        const double b = 0.0;
        const double c = length * (slope_parameter * limited_slope + const_parameter);
        const double min_duration = length * 3.6 / max_speed;
        const double max_duration = length * 3.6 / min_speed;
        assert(min_duration <= max_duration);
        assert(min_duration >= 0);

        assert(std::isfinite(a));
        assert(std::isfinite(b));
        assert(std::isfinite(c));
        assert(std::isfinite(min_duration));
        assert(std::isfinite(max_duration));

        LimitedTradeoffFunction tradeoff;
        if (a > 0)
            tradeoff = {min_duration, max_duration, common::HyperbolicFunction{a, b, c}};
        else
            tradeoff = {min_duration, max_duration, common::ConstantFunction{c}};

        // Only return a tradeoff function of the min and max speed are different
        if (max_speed - min_speed <= 1 || max_duration - min_duration < 1) {
            auto constant_consumption = tradeoff(tradeoff.min_x);
            return LimitedTradeoffFunction{min_duration, min_duration, common::ConstantFunction{constant_consumption}};
        } else {
            return tradeoff;
        }
    }

  private:
    double speed_parameter;
    double slope_parameter;
    double const_parameter;
};

std::unique_ptr<ConsumptionModel> make_consumption_model(PHEMVehicleType type) {
    double speed_parameter, slope_parameter, const_parameter;

    switch (type) {
    case PHEMVehicleType::PEUGEOT_ION:
        speed_parameter = 0.00001084948;
        slope_parameter = 0.02863728;
        const_parameter = 0.08052179;
        break;
    case PHEMVehicleType::EV_NO_AUX:
        speed_parameter = 0.00001129197;
        slope_parameter = 0.05127720;
        const_parameter = 0.1247448;
        break;
    case PHEMVehicleType::EV_SPRING:
        speed_parameter = 0.00001105337;
        slope_parameter = 0.05135052;
        const_parameter = 0.1287824;
        break;
    case PHEMVehicleType::EV_SUMMER:
        speed_parameter = 0.000009192936;
        slope_parameter = 0.05194433;
        const_parameter = 0.1605070;
        break;
    case PHEMVehicleType::EV_WINTER:
        speed_parameter = 0.000004730422;
        slope_parameter = 0.05345339;
        const_parameter = 0.2379125;
        break;
    default:
        throw std::runtime_error("Unknown vehicle type");
    }

    return std::make_unique<PHEMConsumptionModel>(speed_parameter, slope_parameter,
                                                  const_parameter);
}
}

std::unordered_map<std::string, std::unique_ptr<ConsumptionModel>>
make_phem_consumption_models() {
    std::unordered_map<std::string, std::unique_ptr<ConsumptionModel>> models;

    models["Peugeot Ion"] =
        detail::make_consumption_model(detail::PHEMVehicleType::PEUGEOT_ION);
    models["EV No Aux"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_NO_AUX);
    models["EV Spring"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_SPRING);
    models["EV Summer"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_SUMMER);
    models["EV Winter"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_WINTER);

    return models;
}
}

#endif


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

#include "eigengeometry.hpp"

#include <Eigen/Geometry>

namespace VideoStitch {
namespace Calibration {

#ifndef __clang_analyzer__  // VSA-7040
void rotationFromEulerZXY(Eigen::Matrix3d& R, double yaw, double pitch, double roll) {
  Eigen::AngleAxisd X(-pitch, Eigen::Vector3d::UnitX());
  Eigen::AngleAxisd Y(-yaw, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd Z(-roll, Eigen::Vector3d::UnitZ());

  R = Z.matrix() * X.matrix() * Y.matrix();
}
#endif  // __clang_analyzer__

void EulerZXYFromRotation(Eigen::Vector3d& vr, const Eigen::Matrix3d& R) {
  vr(0) = atan2(R(2, 0), R(2, 2));
  vr(1) = -asin(R(2, 1));
  vr(2) = atan2(R(0, 1), R(1, 1));
}

}  // namespace Calibration
}  // namespace VideoStitch