#include "teca_latitude_damper.h"

#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh.h"
#include "teca_string_util.h"

#include <iostream>
#include <set>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

#include <complex.h>

using std::cerr;
using std::endl;

//#define TECA_DEBUG
namespace {

// get the filter ready to be applied in the next steps
template <typename coord_t>
void get_lat_filter(
    coord_t *filter, const coord_t *lat, size_t n_lat_vals,
    coord_t mu, coord_t sigma)
{
    coord_t two_sigma_sqr = 2.0*sigma*sigma;
    for (size_t i = 0; i < n_lat_vals; ++i)
    {
        coord_t x_min_mu = lat[i] - mu;
        coord_t neg_x_min_mu_sqr = -x_min_mu*x_min_mu;
        filter[i] = coord_t(1) - exp(neg_x_min_mu_sqr/two_sigma_sqr);
    }
}

// damp the input array using inverted gaussian
template <typename num_t, typename coord_t>
void apply_lat_filter(
    num_t *output, const num_t *input, const coord_t *filter,
    size_t n_lat_vals, size_t n_lon_vals)
{
    for (size_t j = 0; j < n_lat_vals; ++j)
    {
        size_t jj = j * n_lon_vals;
        for (size_t i = 0; i < n_lon_vals; ++i)
        {
            output[jj + i] = filter[j] * input[jj + i];
        }
    }
}

};

// --------------------------------------------------------------------------
teca_latitude_damper::teca_latitude_damper() :
    center(std::numeric_limits<double>::quiet_NaN()),
    half_width_at_half_max(std::numeric_limits<double>::quiet_NaN()),
    variable_post_fix("")
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_latitude_damper::~teca_latitude_damper()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_latitude_damper::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_latitude_damper":prefix));

    opts.add_options()
        TECA_POPTS_GET(double, prefix, center,
            "set the center (mu) for the gaussian filter")
        TECA_POPTS_GET(double, prefix, half_width_at_half_max,
            "set the value of the half width at half maximum (HWHM) "
            "to calculate sigma from: sigma = HWHM/std::sqrt(2.0*std::log(2.0))")
        TECA_POPTS_MULTI_GET(std::vector<std::string>, prefix, damped_variables,
            "set the variables that will be damped by the inverted "
            "gaussian filter")
        TECA_POPTS_GET(std::string, prefix, variable_post_fix,
            "set the post-fix that will be attached to the variables "
            "that will be saved in the output")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}
// --------------------------------------------------------------------------
void teca_latitude_damper::set_properties(const std::string &prefix,
    variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, double, prefix, center)
    TECA_POPTS_SET(opts, double, prefix, half_width_at_half_max)
    TECA_POPTS_SET(opts, std::vector<std::string>, prefix, damped_variables)
    TECA_POPTS_SET(opts, std::string, prefix, variable_post_fix)
}
#endif

// --------------------------------------------------------------------------
int teca_latitude_damper::get_sigma(const teca_metadata &request, double &sigma)
{
    double hwhm = 0.0;
    if (std::isnan(this->half_width_at_half_max))
    {
        if (request.has("half_width_at_half_max"))
            request.get("half_width_at_half_max", hwhm);
        else
            return -1;
    }
    else
    {
        hwhm = this->half_width_at_half_max;
    }

    sigma = hwhm/std::sqrt(2.0*std::log(2.0));

    return 0;
}

// --------------------------------------------------------------------------
int teca_latitude_damper::get_mu(const teca_metadata &request, double &mu)
{
    if (std::isnan(this->center))
    {
        if (request.has("center"))
            request.get("center", mu);
        else
            return -1;
    }
    else
    {
        mu = this->center;
    }

    return 0;
}

// --------------------------------------------------------------------------
int teca_latitude_damper::get_damped_variables(std::vector<std::string> &vars)
{
    if (this->damped_variables.empty())
        return -1;
    else
        vars = this->damped_variables;

    return 0;
}

// --------------------------------------------------------------------------
teca_metadata teca_latitude_damper::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_latitude_damper::get_output_metadata" << endl;
#endif
    (void)port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);

    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        std::vector<std::string> &damped_vars = this->damped_variables;

        size_t n_arrays = damped_vars.size();
        for (size_t i = 0; i < n_arrays; ++i)
        {
            out_md.append("variables", damped_vars[i] + var_post_fix);
        }
    }

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_latitude_damper::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_latitude_damper::get_upstream_request" << endl;
#endif
    (void) port;
    (void) input_md;

    std::vector<teca_metadata> up_reqs;
    teca_metadata req(request);

    // get the name of the array to request
    std::vector<std::string> damped_vars;
    if (this->get_damped_variables(damped_vars))
    {
        TECA_FATAL_ERROR("No variables to damp specified")
        return up_reqs;
    }

    // pass the incoming request upstream, and
    // add in what we need
    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    arrays.insert(damped_vars.begin(), damped_vars.end());

    // Cleaning off the postfix for arrays passed in the pipeline.
    // For ex a down stream could request "foo_damped" then we'd
    // need to request "foo". also remove "foo_damped" from the
    // request.
    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        teca_string_util::remove_post_fix(arrays, var_post_fix);
    }

    req.set("arrays", arrays);

    // send up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_latitude_damper::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id() << "teca_latitude_damper::execute" << endl;
#endif

    (void)port;

    // get the input
    const_p_teca_cartesian_mesh in_mesh =
        std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("empty input, or not a mesh")
        return nullptr;
    }

    // create output and copy metadata, coordinates, etc
    p_teca_cartesian_mesh out_mesh =
        std::dynamic_pointer_cast<teca_cartesian_mesh>(in_mesh->new_instance());

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    // get the input array names
    std::vector<std::string> damped_vars;
    if (this->get_damped_variables(damped_vars))
    {
        TECA_FATAL_ERROR("No variable specified to damp")
        return nullptr;
    }

    // get Gaussian paramters. if none were provided, these are the defaults
    // that will be used.
    double mu = 0.0;
    double sigma = 45.0;

    this->get_mu(request, mu);
    this->get_sigma(request, sigma);

    // get the coordinate axes
    const_p_teca_variant_array lat = in_mesh->get_y_coordinates();
    const_p_teca_variant_array lon = in_mesh->get_x_coordinates();

    size_t n_lat = lat->size();
    size_t n_lon = lon->size();

    p_teca_variant_array filter_array = lat->new_instance(n_lat);

    // Get the gaussian filter
    NESTED_TEMPLATE_DISPATCH_FP(teca_variant_array_impl,
        filter_array.get(),
        _COORD,

        const NT_COORD *p_lat = static_cast<const TT_COORD*>(lat.get())->get();

        NT_COORD *filter = (NT_COORD*)malloc(n_lat*sizeof(NT_COORD));
        ::get_lat_filter<NT_COORD>(filter, p_lat, n_lat, mu, sigma);

        size_t n_arrays = damped_vars.size();
        for (size_t i = 0; i < n_arrays; ++i)
        {
            // get the input array
            const_p_teca_variant_array input_array
                = out_mesh->get_point_arrays()->get(damped_vars[i]);
            if (!input_array)
            {
                TECA_FATAL_ERROR("damper variable \"" << damped_vars[i]
                    << "\" is not in the input")
                return nullptr;
            }

            // apply the gaussian damper
            size_t n_elem = input_array->size();
            p_teca_variant_array damped_array = input_array->new_instance(n_elem);

            NESTED_TEMPLATE_DISPATCH(teca_variant_array_impl,
                damped_array.get(),
                _DATA,

                const NT_DATA *p_in = static_cast<const TT_DATA*>(input_array.get())->get();
                NT_DATA *p_damped_array = static_cast<TT_DATA*>(damped_array.get())->get();

                ::apply_lat_filter(p_damped_array, p_in, filter, n_lat, n_lon);
            )

            // set the damped array in the output
            std::string out_var_name = damped_vars[i] + this->variable_post_fix;
            out_mesh->get_point_arrays()->set(out_var_name, damped_array);
        }

        free(filter);
    )

    teca_metadata &omd = out_mesh->get_metadata();
    omd.set("gaussian_filter_hwhm", sigma);
    omd.set("gaussian_filter_center_lat", mu);

    return out_mesh;
}


///////////////////////////////////////////////////////////////////////////////
// variance.hpp
//
//  Copyright 2005 Daniel Egloff, Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_VARIANCE_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_VARIANCE_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    //! Lazy calculation of variance.
    /*!
        Default sample variance implementation based on the second moment \f$ M_n^{(2)} \f$ moment<2>, mean and count.
        \f[
            \sigma_n^2 = M_n^{(2)} - \mu_n^2.
        \f]
        where
        \f[
            \mu_n = \frac{1}{n} \sum_{i = 1}^n x_i.
        \f]
        is the estimate of the sample mean and \f$n\f$ is the number of samples.
    */
    template<typename Sample, typename MeanFeature>
    struct lazy_variance_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        lazy_variance_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            extractor<MeanFeature> mean;
            result_type tmp = mean(args);
            return accumulators::moment<2>(args) - tmp * tmp;
        }
    };

    //! Iterative calculation of variance.
    /*!
        Iterative calculation of sample variance \f$\sigma_n^2\f$ according to the formula
        \f[
            \sigma_n^2 = \frac{1}{n} \sum_{i = 1}^n (x_i - \mu_n)^2 = \frac{n-1}{n} \sigma_{n-1}^2 + \frac{1}{n-1}(x_n - \mu_n)^2.
        \f]
        where
        \f[
            \mu_n = \frac{1}{n} \sum_{i = 1}^n x_i.
        \f]
        is the estimate of the sample mean and \f$n\f$ is the number of samples.

        Note that the sample variance is not defined for \f$n <= 1\f$.

        A simplification can be obtained by the approximate recursion
        \f[
            \sigma_n^2 \approx \frac{n-1}{n} \sigma_{n-1}^2 + \frac{1}{n}(x_n - \mu_n)^2.
        \f]
        because the difference
        \f[
            \left(\frac{1}{n-1} - \frac{1}{n}\right)(x_n - \mu_n)^2 = \frac{1}{n(n-1)}(x_n - \mu_n)^2.
        \f]
        converges to zero as \f$n \rightarrow \infty\f$. However, for small \f$ n \f$ the difference
        can be non-negligible.
    */
    template<typename Sample, typename MeanFeature, typename Tag>
    struct variance_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        template<typename Args>
        variance_impl(Args const &args)
          : variance(numeric::fdiv(args[sample | Sample()], numeric::one<std::size_t>::value))
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if(cnt > 1)
            {
                extractor<MeanFeature> mean;
                result_type tmp = args[parameter::keyword<Tag>::get()] - mean(args);
                this->variance =
                    numeric::fdiv(this->variance * (cnt - 1), cnt)
                  + numeric::fdiv(tmp * tmp, cnt - 1);
            }
        }

        result_type result(dont_care) const
        {
            return this->variance;
        }

    private:
        result_type variance;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::variance
// tag::immediate_variance
//
namespace tag
{
    struct lazy_variance
      : depends_on<moment<2>, mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::lazy_variance_impl<mpl::_1, mean> impl;
    };

    struct variance
      : depends_on<count, immediate_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::variance_impl<mpl::_1, mean, sample> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::lazy_variance
// extract::variance
//
namespace extract
{
    extractor<tag::lazy_variance> const lazy_variance = {};
    extractor<tag::variance> const variance = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(lazy_variance)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(variance)
}

using extract::lazy_variance;
using extract::variance;

// variance(lazy) -> lazy_variance
template<>
struct as_feature<tag::variance(lazy)>
{
    typedef tag::lazy_variance type;
};

// variance(immediate) -> variance
template<>
struct as_feature<tag::variance(immediate)>
{
    typedef tag::variance type;
};

// for the purposes of feature-based dependency resolution,
// immediate_variance provides the same feature as variance
template<>
struct feature_of<tag::lazy_variance>
  : feature_of<tag::variance>
{
};

// So that variance can be automatically substituted with
// weighted_variance when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::variance>
{
    typedef tag::weighted_variance type;
};

// for the purposes of feature-based dependency resolution,
// weighted_variance provides the same feature as variance
template<>
struct feature_of<tag::weighted_variance>
  : feature_of<tag::variance>
{
};

// So that immediate_variance can be automatically substituted with
// immediate_weighted_variance when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::lazy_variance>
{
    typedef tag::lazy_weighted_variance type;
};

// for the purposes of feature-based dependency resolution,
// immediate_weighted_variance provides the same feature as immediate_variance
template<>
struct feature_of<tag::lazy_weighted_variance>
  : feature_of<tag::lazy_variance>
{
};

////////////////////////////////////////////////////////////////////////////
//// droppable_accumulator<variance_impl>
////  need to specialize droppable lazy variance to cache the result at the
////  point the accumulator is dropped.
///// INTERNAL ONLY
/////
//template<typename Sample, typename MeanFeature>
//struct droppable_accumulator<impl::variance_impl<Sample, MeanFeature> >
//  : droppable_accumulator_base<
//        with_cached_result<impl::variance_impl<Sample, MeanFeature> >
//    >
//{
//    template<typename Args>
//    droppable_accumulator(Args const &args)
//      : droppable_accumulator::base(args)
//    {
//    }
//};

}} // namespace boost::accumulators

#endif


/* Copyright (C) 5/23/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef STAT_ACC_HPP_
#define STAT_ACC_HPP_

#include <deque>

#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/kurtosis.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <Eigen/Dense>

#include <algorithm>
#include <vector>

namespace MCUtil
{

    /*!
     * @brief Statistic types to be collected
     */
enum class StatType : unsigned char {
    MEAN = 0,
    VARIANCE  = 1
    //,SKEWNESS = 2,
    //KURTOSIS = 3
};

//use cache with sfinae to only define caching vector if needed

    /*!
     * @brief               This class provides the low level accumulation functionality for statistical quantities of observables
     * @tparam T            Type of quantity
     * @tparam CACHE_SIZE   Cache for the last CACHE_SIZE samples. This can be used for quantities that depend on multiple samples
     */
template<typename T, unsigned long CACHE_SIZE = 0>
class StatAcc
{

    using AccT = boost::accumulators::accumulator_set<T,
            boost::accumulators::features<
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance
                    //,boost::accumulators::tag::skewness
                    //,boost::accumulators::tag::kurtosis
            > >;

private:
    AccT acc;
    //@TODO: switch to boost circular buffer to store full time series?
    std::deque<T> cache;
    unsigned long cache_used;

public:
    /*StatAcc(): cache_used{0}
    {
        if(CACHE_SIZE)
            cache.resize(CACHE_SIZE);
    }
     */

    StatAcc &operator=(const StatAcc &) = delete;
    //StatAcc(const StatAcc&) = delete;

    /*!
     * @brief
     * @tparam ArgTypes     Argument types for the constructor of the quantity type to be accumulated
     * @param args          Arguments for the constructor of the quantity type to be accumulated
     */
    template<typename... ArgTypes>
    StatAcc(ArgTypes&&... args):
            acc(T(std::forward<ArgTypes>(args)...)), cache_used{0} {
        if(CACHE_SIZE)
            cache.resize(CACHE_SIZE);
    }

    friend std::ostream &operator<<(std::ostream &stream, const StatAcc &sa) {
        stream << "This is the temporary output for StatAcc";
        return stream;
    }

    /*!
     * @brief       Adds another sample to the accumulator
     * @param val   Value of next sample
     */
    void operator()(T val) {
        if(CACHE_SIZE) {
            if(cache_used < CACHE_SIZE) {
                cache.push_back(val);
                cache_used += 1;
            } else {
                cache.pop_front();
                cache.push_back(val);
            }
        }
        acc(val);
    }

    /*!
     * @brief       extracts statistic of accumulated quantity
     * @param st    Stat Type
     * @return      Value of statistic for quantity
     */
    T extract(const StatType st) {
        T res;
        switch(st) {
            case StatType::MEAN:
                res = boost::accumulators::mean(acc);
                break;
            case StatType::VARIANCE:
                res = boost::accumulators::variance(acc);
                break;
                /* @TODO: template disable
                 * case StatType::SKEWNESS:
                    res = boost::accumulators::variance(acc);
                    break;
                case StatType::KURTOSIS:
                    res = boost::accumulators::kurtosis(acc);
                    break;
                    */
        }
        return res;
    }
};

// ===== Eigen::MatrixXd specialization

    // TODO: do this as template specialization
    template<typename T, unsigned long CACHE_SIZE = 0>
    class StatAccEigen
    {

        using AccT = boost::accumulators::accumulator_set<std::vector<T>,
                boost::accumulators::features<
                        boost::accumulators::tag::mean,
                        boost::accumulators::tag::variance
                        //,boost::accumulators::tag::skewness
                        //,boost::accumulators::tag::kurtosis
                > >;

    private:
        AccT acc;
        std::vector<T> mapped;
        std::deque<std::vector<T>> cache;
        int r;
        int c;
        unsigned long cache_used;

    public:

        StatAccEigen &operator=(const StatAccEigen &) = delete;

        /*!
         * @brief
         * @param rows     Rows of matrices to be accumulated
         * @param cols     Cols of matrices to be accumulated
         */
        StatAccEigen(int rows, int cols):
                acc(std::vector<T>(rows*cols)), cache_used{0}, r(rows), c(cols)
        {
                mapped.resize(cols*rows);
                if(CACHE_SIZE)
                cache.resize(CACHE_SIZE);
        }
        /*!
         * @brief       Adds another sample to the accumulator
         * @param val   Value of next sample
         */
        void new_sample(const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>& val) {
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Map(&mapped[0], val.rows(), val.cols()) = val;
            if(CACHE_SIZE) {
                if(cache_used < CACHE_SIZE) {
                    cache.push_back(mapped);
                    cache_used += 1;
                } else {
                    cache.pop_front();
                    cache.push_back(mapped);
                }
            }
            acc(mapped);
        }

        /*!
         * @brief       Adds another sample to the accumulator
         * @param val   Value of next sample
         */
        void operator()(const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>& val) {
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Map(&mapped[0], val.rows(), val.cols()) = val;
            if(CACHE_SIZE) {
                if(cache_used < CACHE_SIZE) {
                    cache.push_back(mapped);
                    cache_used += 1;
                } else {
                    cache.pop_front();
                    cache.push_back(mapped);
                }
            }
            acc(mapped);
        }

        /*!
         * @brief       extracts statistic of accumulated quantity
         * @param st    Stat Type
         * @return      Value of statistic for quantity
         */
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> extract(const StatType st) {
            std::vector<T> res;
            switch(st) {
                case StatType::MEAN:
                    res = boost::accumulators::mean(acc);
                    break;
                case StatType::VARIANCE:
                    res = boost::accumulators::variance(acc);
                    break;
                    /* @TODO: template disable
                     * case StatType::SKEWNESS:
                        res = boost::accumulators::variance(acc);
                        break;
                    case StatType::KURTOSIS:
                        res = boost::accumulators::kurtosis(acc);
                        break;
                        */
            }
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> out = \
                Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>(&res[0], r, c);
            return out;
        }
    };

} // end namespace

#endif


/****************************************************************************
**
** Copyright (C) 2016 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of the Qt Charts module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:GPL$
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see https://www.qt.io/terms-conditions. For further
** information use the contact form at https://www.qt.io/contact-us.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3 or (at your option) any later version
** approved by the KDE Free Qt Foundation. The licenses are as published by
** the Free Software Foundation and appearing in the file LICENSE.GPL3
** included in the packaging of this file. Please review the following
** information to ensure the GNU General Public License requirements will
** be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "view.h"
#include <QtGui/QResizeEvent>
#include <QtWidgets/QGraphicsScene>
#include <QtCharts/QChart>
#include <QtCharts/QLineSeries>
#include <QtCharts/QSplineSeries>
#include <QtWidgets/QGraphicsTextItem>
#include "callout.h"
#include <QtGui/QMouseEvent>
#include <Eigen/Dense>
#include "kalman.h"

View::View(QWidget *parent)
    : QGraphicsView(new QGraphicsScene, parent),
      m_coordX(0),
      m_coordY(0),
      m_chart(0),
      m_tooltip(0)
{
    int n = 9; // Number of states
    int m = 3; // Number of measurements
    unsigned int dt = 5.; // delta
    unsigned int endTime = 60;
    Eigen::MatrixXd A(n, n); // System dynamics matrix (Transformation Matrix)
    Eigen::MatrixXd C(m, n); // Output matrix
    Eigen::MatrixXd Q(n, n); // Process noise covariance
    Eigen::MatrixXd R(m, m); // Measurement noise covariance
    Eigen::MatrixXd P(n, n); // Estimate error covariance
    // Discrete LTI projectile motion, measuring position only
    double tt = 0.5 * dt * dt;
    A << 1, 0, 0, dt,  0,  0, tt,  0,  0,
         0, 1, 0,  0, dt,  0,  0, tt,  0,
         0, 0, 1,  0,  0, dt,  0,  0, dt,
         0, 0, 0,  1,  0,  0, dt,  0,  0,
         0, 0, 0,  0,  1,  0,  0, dt,  0,
         0, 0, 0,  0,  0,  1,  0,  0, dt,
         0, 0, 0,  0,  0,  0,  1,  0,  0,
         0, 0, 0,  0,  0,  0,  0,  1,  0,
         0, 0, 0,  0,  0,  0,  0,  0,  1;
    C << 1, 0, 0, 0, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0, 0, 0, 0,
         0, 0, 1, 0, 0, 0, 0, 0, 0;
    // Reasonable covariance matrices
    Q << tt, .05, .05, .0, .0, .0, .0, .0, .0,
         .05, tt, .05, .0, .0, .0, .0, .0, .0,
         .05, .5, tt, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0,
         .0, .0, .0, .0, .0, .0, .0, .0, .0;
    R << 5, 0, 0,
         0, 5, 0,
         0, 0, 5;
    P << 1.3, 2, 3, 1, 1, 1, 1, 1, 1,
         4, 1.5, 5, 1, 1, 1, 1, 1, 1,
         6, 7, 9, 1, 1, 1, 1, 1, 1,
         1, 1, 1, 1.6, 1, 1, 1, 1, 1,
         1, 1, 1, 1, .9, 1, 1, 1, 1,
         1, 1, 1, 1, 1, .2, 1, 1, 1,
         1, 1, 1, 1, 1, 1, .1, 1, 1,
         1, 1, 1, 1, 1, 1, 1, 1.54, 1,
         1, 1, 1, 1, 1, 1, 1, 1, 1.2;
    KalmanFilter k(dt, A, C, Q, R, P);
    Eigen::VectorXd s0(n), y(m);
    s0 <<  7.0,  8.0,  9.0, 1.0,  2.0,  3.0, 0.33, 0.66, 0.99;
    //      px,   py,   pz,  vx,   vy,   vz,   ax,   ay,   az
    k.init(0, s0);

    setDragMode(QGraphicsView::NoDrag);
    setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
    setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);

    // chart
    m_chart = new QChart;
    m_chart->setMinimumSize(640, 480);
    m_chart->setTitle("Distance to Origin (m) x Instant (s)");
    //m_chart->legend()->hide();
    m_chart->legend()->setVisible(true);
    m_chart->legend()->setAlignment(Qt::AlignBottom);

    QLineSeries *series = new QLineSeries;
    series->setName("Observed");
    QSplineSeries *series2 = new QSplineSeries;
    series2->setName("Predicted");
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(n, n);
    for (unsigned int t = 0; t <= endTime; t += dt)
    {
        Eigen::VectorXd u = Eigen::VectorXd::Random(n);
        Eigen::VectorXd y_ = A * s0 + B * u;
        Eigen::Vector3d v;
        v << y_[0], y_[1], y_[2];
        k.update(v);
        series->append(t, v.norm());// distance to origin
        series2->append(t, k.state().norm()); // distance to origin
        s0 = y_;
    }
    m_chart->addSeries(series);
    m_chart->addSeries(series2);

    m_chart->createDefaultAxes();
    m_chart->setAcceptHoverEvents(true);

    setRenderHint(QPainter::Antialiasing);
    scene()->addItem(m_chart);

    m_coordX = new QGraphicsSimpleTextItem(m_chart);
    m_coordX->setPos(m_chart->size().width()/2 - 50, m_chart->size().height());
    m_coordX->setText("X: ");
    m_coordY = new QGraphicsSimpleTextItem(m_chart);
    m_coordY->setPos(m_chart->size().width()/2 + 50, m_chart->size().height());
    m_coordY->setText("Y: ");

    connect(series, &QLineSeries::clicked, this, &View::keepCallout);
    connect(series, &QLineSeries::hovered, this, &View::tooltip);

    connect(series2, &QSplineSeries::clicked, this, &View::keepCallout);
    connect(series2, &QSplineSeries::hovered, this, &View::tooltip);

    this->setMouseTracking(true);
}

void View::resizeEvent(QResizeEvent *event)
{
    if (scene()) {
        scene()->setSceneRect(QRect(QPoint(0, 0), event->size()));
         m_chart->resize(event->size());
         m_coordX->setPos(m_chart->size().width()/2 - 50, m_chart->size().height() - 20);
         m_coordY->setPos(m_chart->size().width()/2 + 50, m_chart->size().height() - 20);
         const auto callouts = m_callouts;
         for (Callout *callout : callouts)
             callout->updateGeometry();
    }
    QGraphicsView::resizeEvent(event);
}

void View::mouseMoveEvent(QMouseEvent *event)
{
    m_coordX->setText(QString("X: %1").arg(m_chart->mapToValue(event->pos()).x()));
    m_coordY->setText(QString("Y: %1").arg(m_chart->mapToValue(event->pos()).y()));
    QGraphicsView::mouseMoveEvent(event);
}

void View::keepCallout()
{
    m_callouts.append(m_tooltip);
    m_tooltip = new Callout(m_chart);
}

void View::tooltip(QPointF point, bool state)
{
    if (m_tooltip == 0)
        m_tooltip = new Callout(m_chart);

    if (state) {
        m_tooltip->setText(QString("X: %1 \nY: %2 ").arg(point.x()).arg(point.y()));
        m_tooltip->setAnchor(point);
        m_tooltip->setZValue(11);
        m_tooltip->updateGeometry();
        m_tooltip->show();
    } else {
        m_tooltip->hide();
    }
}


// A note to a potential future debugger, the -fno-use-cxa-atexit is needed solely for lli (the llvm interpreter) to work
//   Actually compiling it works fine, and moreover, the one place atexit is used is nowhere near the enzyme code

// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -mllvm -force-vector-width=1 -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O3 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O2 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -Xclang -new-struct-path-tbaa -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
//   note not doing O0 below as to ensure we get tbaa
// RUN: %clang++ -Xclang -new-struct-path-tbaa -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 -Xclang -disable-llvm-optzns %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O3 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O2 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// Note the below ends up with a memcpy from undefined memory data for type analysis to handle
// RUN: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O1 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -
// TODO: %clang++ -fno-use-cxa-atexit -ffast-math -fno-unroll-loops -fno-vectorize -fno-slp-vectorize -fno-exceptions -O0 %s -S -emit-llvm -o - | %opt - %loadEnzyme -enzyme -enzyme-inline=1 -S | %lli -

#include "test_utils.h"

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>
void boost::throw_exception(std::exception const & e){
    //do nothing
}

using namespace std;
using namespace boost::numeric::odeint;

#include <stdio.h>

typedef boost::array< double , 1 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    const double a = 1.2;
    dxdt[0] = -a * x[0];
}

double foobar(double t=10.0) {
    state_type x = { 1.0 }; // initial conditions

    typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/100 );

    //printf("final result t=%f x(t)=%f, exp(-1.2* t)=%f\n", t, x[0], exp(- 1.2 * t));
    return x[0];
}

extern "C" {
extern double __enzyme_autodiff(void*, double);
}

int main(int argc, char **argv)
{
    for(int i=1; i<=100; i++) {
        double t=i/10.;
        double res = __enzyme_autodiff((void*)foobar, t);
        double realanswer = -1.2*exp(-1.2*t);
        printf("t=%f d/dt(exp(-1.2*t))=%f, -1.2*exp(-1.2*t)=%f\n", t, res, realanswer);
        // see if approximation is within 10%
        APPROX_EQ(res, realanswer, max(fabs(realanswer)/10., 2.0e-5) );
    }
}


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

/*
 Copyright (C) 2014 Jose Aparicio

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

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

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

#ifndef quantlib_math_multidimintegrator_hpp
#define quantlib_math_multidimintegrator_hpp

#include <vector>

#include <boost/function.hpp>
#include <boost/bind.hpp>

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <ql/math/integrals/integral.hpp>

namespace QuantLib {

    /*! \brief Integrates a vector or scalar function of vector domain.

        Uses a collection of arbitrary 1D integrators along each of the
        dimensions. A template recursion along dimensions avoids calling depth
        test or virtual functions.\par
        This class generalizes to an arbitrary number of dimensions the
        functionality in class TwoDimensionalIntegral
    */
    class MultidimIntegral {
    public:
        explicit MultidimIntegral(
            const std::vector<boost::shared_ptr<Integrator> >& integrators);

        // scalar variant:
        /*!
            @param f Integrand function.
            @param a Lower integration limit domain for each dimension.
            @param b Upper integration limit domain for each dimension.
        */
        Real operator()(
            const boost::function<Real (const std::vector<Real>&)>& f,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const
        {
            QL_REQUIRE((a.size()==b.size())&&(b.size()==integrators_.size()),
                "Incompatible integration problem dimensions");
            return integrationLevelEntries_[integrators_.size()-1](f, a, b);
        }
        // to do: write std::vector<Real> operator()(...) version

    private:
        static const Size maxDimensions_ = 15;

        /* Here is the tradeoff; this is avoiding the dimension limits checks
        during integration at the price of these asignments during construction.
        Explicit template instantiation is of no use, an object is needed
        (notice 'this' is needed for the asignment.)
        If not all the dimensions up the maximum number are used the waste goes
        into storage of the functions (in fact only one is used)
        */
        template<Size depth>
        void spawnFcts() const;
        // Splits the integration in cross-sections per dimension.
        template<int T_N>
        Real vectorBinder (
            const boost::function<Real (const std::vector<Real>&)>& f,
            Real z,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const ;
        // actual integration of dimension nT
        template<int nT>
        Real integrate(
            const boost::function<Real (const std::vector<Real>&)>& f,
            const std::vector<Real>& a,
            const std::vector<Real>& b) const;
    private:
        const std::vector<boost::shared_ptr<Integrator> > integrators_;

        /* typedef (const boost::function<Real
            (const std::vector<Real>&arg1)>&arg2) integrableFunctType;
        */

        /* vector of, functions returning reals And taking as argument:
        1.- a const ref to a function taking vectors
        2.- a vector, 3. another vector. typedefs eventually...
         at first sight this might look like mimicking a virtual table, it isnt
         that. The reason is to be able to select the correct integration
         dimension at run time, this can not be done before because of the
         template argument restriction to be constant known at compilation.
        */
        mutable std::vector<boost::function<Real (//<- members: integrate<N>
            // integrable function:
            const boost::function<Real (const std::vector<Real>&)>&,
            const std::vector<Real>&, //<- a
            const std::vector<Real>&) //<- b
            > >
            integrationLevelEntries_;

        /* One can avoid the passing around of the ct refs to a and b but the
        price is to keep a copy of them (they are unknown at construction time)
         On the other hand the vector integration variable has to be created.*/
        mutable std::vector<Real> varBuffer_;

    };

    // spez last call/dimension
    template<>
    Real inline MultidimIntegral::vectorBinder<0> (
        const boost::function<Real (const std::vector<Real>&)>& f,
        Real z,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        varBuffer_[0] = z;
        return f(varBuffer_);
    }

    template<>
    void inline MultidimIntegral::spawnFcts<1>() const {
        integrationLevelEntries_[0] =
            boost::bind(&MultidimIntegral::integrate<0>, this, _1, _2, _3);
    }

	template<int nT>
	inline Real MultidimIntegral::integrate(
        const boost::function<Real (const std::vector<Real>&)>& f,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        return
            (*integrators_[nT])(
                boost::bind(&MultidimIntegral::vectorBinder<nT>, this, f,
                    _1, boost::cref(a), boost::cref(b)), a[nT], b[nT]);
    }

    template<int T_N>
    inline Real MultidimIntegral::vectorBinder (
        const boost::function<Real (const std::vector<Real>&)>& f,
        Real z,
        const std::vector<Real>& a,
        const std::vector<Real>& b) const
    {
        varBuffer_[T_N] = z;
        return integrate<T_N-1>(f, a, b);
    }

    template<Size depth>
    void MultidimIntegral::spawnFcts() const {
        integrationLevelEntries_[depth-1] =
          boost::bind(&MultidimIntegral::integrate<depth-1>, this,
            _1, _2, _3);
        spawnFcts<depth-1>();
    }

}

#endif


#ifndef __FOLDING_ESTIMATION_ALGORITHM__
#define __FOLDING_ESTIMATION_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{

  /**
    Class that implements the contact point estimator
    used in the folding assembly problem.
  **/
  class FoldingAssemblyEstimator : public AlgorithmBase
  {
  public:
    FoldingAssemblyEstimator();
    ~FoldingAssemblyEstimator();

    /**
      Computes an estimate of the free vector that represents a rigidly grasped object.

      Makes use of the process model \f$\dot{\mathbf{r}} = \mathbf{S}(\boldsymbol{\omega_1}}) \mathbf{r}\f$
      and of the observation model \f$\boldsymbol{\tau} = -\mathbf{S}(\mathbf{f})\mathbf{r}\f$.

      @param omega The end-effector angular velocity.
      @param force The measured contact force.
      @param torque The measured torque at the end-effector wrist.
      @param dt Elapsed time between calls (in seconds).
      @return A free vector representing the estimate of the vector that connects the wrench measurement point to the contact point.
    **/
    Eigen::Vector3d estimate(const Eigen::Vector3d &omega, const Eigen::Vector3d &force, const Eigen::Vector3d &torque, const double dt);

    /**
      Initialize the estimator

      @param r The initial virtual stick estimate.
    **/
    void initialize(const Eigen::Vector3d &r);

    virtual bool getParams(const ros::NodeHandle &n);

  private:
    Eigen::Vector3d r_;
    Eigen::Matrix3d P_, Q_, R_;
  };
}
#endif


#include "ParallelEigenvectorsEvaluator.hh"

#include <Eigen/Geometry>

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

#include <vector>

using namespace cpp_utils;

namespace tl
{

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

std::array<TPBT<double, 1, 2>, 6>
parallelEigenvectorsCoeffs(const TensorInterp& s,
                           const TensorInterp& t,
                           const Triangle& r)
{
    using Coords = TPBT<double, 1, 2>::Coords;

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

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

    auto eval4 = [&](const Coords& coords) { return eval_ev(coords, t, 0); };
    auto eval5 = [&](const Coords& coords) { return eval_ev(coords, t, 1); };
    auto eval6 = [&](const Coords& coords) { return eval_ev(coords, t, 2); };

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

using PEVE = ParallelEigenvectorsEvaluator;

PEVE::ParallelEigenvectorsEvaluator(const DoubleTri& tri,
                                    const TensorInterp& s,
                                    const TensorInterp& t,
                                    const Options& opts)
        : _tri(tri),
          _target_funcs(parallelEigenvectorsCoeffs(s, t, tri.dir_tri)),
          _opts(opts)
{
}

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

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

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

    // Compute upper bound for target function
    auto max_error = abs_max_upper_bound(_target_funcs);

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

    return Result::Split;
}

double PEVE::error() const
{
    return upper_bound_norm(_target_funcs);
}

double PEVE::condition() const
{
    auto gradients = std::vector<Eigen::Vector4d>{};
    gradients.reserve(_target_funcs.size());

    auto min_cos = 0.;

    auto center0 = (TensorProductDerivativeType_t<0, double, double, 1, 2>::
                            Coords::Ones()
                    / 3.).eval();
    auto center1 = (TensorProductDerivativeType_t<1, double, double, 1, 2>::
                            Coords::Ones()
                    / 3.).eval();

    for(const auto& poly: _target_funcs)
    {
        auto deriv0 = derivatives<0>(poly);
        auto deriv1 = derivatives<1>(poly);
        auto grad = Eigen::Vector4d(deriv0[0](center0),
                                    deriv0[1](center0),
                                    deriv1[0](center1),
                                    deriv1[1](center1));
        grad.normalize();
        for(const auto& g: gradients)
        {
            min_cos = std::min(min_cos, std::abs(grad.dot(g)));
        }
        gradients.push_back(grad);
    }

    auto angle = std::acos(min_cos);
    return min_cos / std::sin(angle);
}

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

bool operator==(const PEVE& t1, const PEVE& t2)
{
    return t1._tri == t2._tri && t1._target_funcs == t2._target_funcs
           && t1._last_split_dir == t2._last_split_dir
           && t1._split_level == t2._split_level && t1._opts == t2._opts;
}

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

}// namespace tl


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

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

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <boost/math/special_functions/bessel.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/vmf.hpp>

using namespace Eigen;
using std::endl;
using std::cout;
using std::vector;

template<typename T>
class vMFpriorFull : public Distribution<T>
{
public:
  vMFpriorFull(const Matrix<T,Dynamic,1>& m0, T t0, T a0, T b0, boost::mt19937
    *pRndGen);
  vMFpriorFull(const vMFpriorFull<T>& vmfPriorFull);
  ~vMFpriorFull();

//  vMFpriorFull<T> posterior(const Matrix<T,Dynamic,Dynamic>& x, const
//      VectorXu& z, uint32_t k);
//  vMFpriorFull<T> posterior() const;

  void resetSufficientStatistics();
  void getSufficientStatistics(const Matrix<T,Dynamic,Dynamic> &x,
    const VectorXu& z, uint32_t k);

  vMF<T> sample();
  vMF<T> sampleFromPosterior(const vMF<T>& vmf);

//  T logPdf(const vMF<T>& vmf) const;
//  T logPdfMarginalized() const; // log pdf of SS under NIW prior
//  T logPdfUnderPriorMarginalizedMerged(const NIW<T>& other) const;

  vMF<T> vmf0_; // prior on the mean
  T a0_;
  T b0_;

  uint32_t D_;
  // sufficient statistics
  Matrix<T,Dynamic,1> xSum_;
  T count_;

  boost::mt19937 *pRndGen_;

private:
  boost::uniform_01<> unif_;

  T concentrationLogPdf(const T tau, const T a, const T b) const;
  T sampleConcentration(const T a, const T b, const T TT=10);
};
// -----------------------------------------------------------------------

template<typename T>
vMFpriorFull<T>::vMFpriorFull(const Matrix<T,Dynamic,1>& m0, T t0, T a0, T
    b0, boost::mt19937 *pRndGen)
: Distribution<T>(pRndGen), vmf0_(m0,t0,pRndGen), a0_(a0), b0_(b0), D_(m0.rows()),
xSum_(Matrix<T,Dynamic,1>::Zero(m0.rows())), count_(0.), pRndGen_(pRndGen)
{
//cout<<m0<<endl; cout<<m0.rows()<<endl;cout<<uint32_t(m0.rows())<<endl;
};

template<typename T>
vMFpriorFull<T>::vMFpriorFull(const vMFpriorFull<T>& vmfPriorFull)
: Distribution<T>(vmfPriorFull.pRndGen_), vmf0_(vmfPriorFull.vmf0_),
a0_(vmfPriorFull.a0_), b0_(vmfPriorFull.b0_), D_(vmfPriorFull.D_),
xSum_(vmfPriorFull.xSum_), count_(vmfPriorFull.count_),
pRndGen_(vmfPriorFull.pRndGen_)
{};

template<typename T>
vMFpriorFull<T>::~vMFpriorFull()
{
};

//template<typename T>
//vMFpriorFull<T> vMFpriorFull<T>::posterior(const
//    Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z, uint32_t k)
//{
//  getSufficientStatistics(x,z,k);
//  return posterior();
//};
//
//template<typename T>
//vMFpriorFull<T> vMFpriorFull<T>::posterior() const
//{
//  Matrix<T,Dynamic,1> xi = t0_*m0_ +;
//  const T a = a0_ + count_;
//  const T b = b0_ +
//  return vMFpriorFull(xSum_, count_);
//};

template<typename T>
void vMFpriorFull<T>::resetSufficientStatistics()
{
  xSum_.setZero(D_);
  count_ = 0;
};

template<typename T>
void vMFpriorFull<T>::getSufficientStatistics(const
    Matrix<T,Dynamic,Dynamic> &x, const VectorXu& z, uint32_t k)
{
  this->resetSufficientStatistics();
  // TODO: be carefull here when parallelizing since all are writing to the same
  // location in memory
#pragma omp parallel for
  for (int32_t i=0; i<z.size(); ++i)
  {
    if(z(i) == k)
    {
#pragma omp critical
      {
        xSum_ += x.col(i);
        count_++;
      }
    }
  }
#ifndef NDEBUG
  cout<<" -- updating ss "<<count_<<endl;
  cout<<"xSum="<<xSum_.transpose()<<endl;
  posterior().print();
#endif
//  cout<<"SS: "<<count_<<" "<<xSum_.transpose()<<endl;
};

template<typename T>
vMF<T> vMFpriorFull<T>::sample()
{
  const T tau = sampleConcentration(a0_,b0_);
  const Matrix<T,Dynamic,1> mu = vmf0_.sample();
  return vMF<T>(mu, tau,pRndGen_);
};

template<typename T>
vMF<T> vMFpriorFull<T>::sampleFromPosterior(const vMF<T>& vmf)
{
  vMF<T> vmfPost (vmf);
  vMF<T> vmfPostPrev (vmf);
  T logPost = -FLT_MAX;
  T logPostPrev = logPost;
  // Gibbs sampler for concentration and mean
  for(uint32_t j=0;j<10; ++j)
  {
    // posterior concentration prior parameters
    const T a = a0_ + count_;
    const T b = b0_ + vmfPost.mu().transpose()*xSum_;
//    cout<<"vMFpriorFull::sampleFromPosterior: a="<<a<<" b="<<b<<endl;
    // sample concentration form this posterior
    const T tau = sampleConcentration(a,b);
//    cout<<"vMFpriorFull::sampleFromPosterior: tau="<<tau<<endl;
    // posterior mean (m_N) and concentration (t_N) for vMF
    Matrix<T,Dynamic,1> m_N = vmf0_.tau()*vmf0_.mu() + tau*xSum_;
    T t_N = m_N.norm();
    m_N /= t_N;
//    cout<<"vMFpriorFull::sampleFromPosterior: t_N="<<t_N<<" m_N="<<m_N.transpose()<<endl;
    // sample mean mu from posterior vMF
    Matrix<T,Dynamic,1> mu = vMF<T>(m_N,t_N, pRndGen_).sample();
//    cout<<"vMFpriorFull::sampleFromPosterior: mu="<<mu.transpose()<<endl;
    logPost = (tau*b + tau*mu.transpose()*xSum_ + vmf0_.tau()*vmf0_.mu().transpose()*mu);
    cout<<"@"<<j<<" cost "<<logPost<<" "<<(logPost - logPostPrev)<<endl;
    // return sampled posterior vMF
    vmfPost = vMF<T>(mu, tau, pRndGen_);
//    if((logPost - logPostPrev)<0. ) break;
    logPostPrev = logPost;
    vmfPostPrev = vmfPost;
  }
  return vmfPostPrev;
};

template<typename T>
T vMFpriorFull<T>::concentrationLogPdf(const T tau, const T a, const T b) const
{
  // modified bessel function of the first kind
  // http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
  const T DD = static_cast<T>(D_);
//  cout<<"vMFpriorFull:: bessel: D="<<DD<<" "<<(DD/2.-1.)<<" tau="<<tau<<endl;
  return a*((DD/2. -1.)*log(tau)
    - (DD/2.)*log(2.*M_PI)
    - logBesselI(DD/2. -1.,tau))
    + tau*b;
};

template<typename T>
T vMFpriorFull<T>::sampleConcentration(const T a, const T b, const T TT)
{
  // slice sampler for concentration paramter tau
  const T w = 0.1;  // width for expansions of search region
  T tau = 0.11;      // arbitrary starting point
  for(int32_t t=0; t<TT; ++t)
  {
    const T yMax = concentrationLogPdf(tau,a,b);
    const T y = log(unif_(*pRndGen_)) + yMax;
//    cout<<"vMFpriorFull::sampleConcentration: yMax="<<yMax<<" y="<<y<<endl;
    T tauMin = tau-w;
    T tauMax = tau+w;
    while (tauMin >=0. && concentrationLogPdf(tauMin,a,b) >= y) tauMin -= w;
    tauMin = max(0.0,tauMin);
    while (concentrationLogPdf(tauMax,a,b) >= y) tauMax += w;
    while(42){
      T tauNew = unif_(*pRndGen_)*(tauMax-tauMin)+tauMin;
//      cout<<"vMFpriorFull::sampleConcentration: "<<tauNew<<" min: "<<tauMin<<" max: "<<tauMax<<" a="<<a<<" b="<<b<<endl;
      if(concentrationLogPdf(tauNew,a,b) >= y)
      {
        tau = tauNew; break;
      }else{
        if (tauNew < tau) tauMin = tauNew; else tauMax = tauNew;
      }
    };
  }
  return tau;
};


#include <vi/ea.h>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <cstdio>
#include <iostream>
#include <random>
#include <string>

// Merciful C++ gods, please absolve me of these sins

#define BUILD_SYSTEM(PARENT_SELECTION, ADULT_SELECTION) \
    vi::ea::build_system( \
        std::default_random_engine{std::random_device{}()}, \
        vi::ea::dynamic_int_vector_creator{ \
            variables["L"].as<unsigned>(), \
            variables["L"].as<unsigned>(), \
            std::uniform_int_distribution<unsigned>{0U, variables["S"].as<unsigned>() - 1U}}, \
        PARENT_SELECTION, \
        vi::ea::reproduction::sexual{ \
            variables["mutation_rate"].as<double>(), \
            variables["crossover_rate"].as<double>(), \
            variables["crossover_points"].as<unsigned>()}, \
        ADULT_SELECTION, \
        variables["population_size"].as<unsigned>(), \
        [=] (const auto& genotype) \
        { \
            unsigned collisions = 0; \
            const auto d_max = variables.count("local") ? 0 : genotype.size() - 3; \
            for (int d = 0; d <= d_max; ++d) \
            { \
                collisions += evaluate_surprising_sequence_collisions(genotype, variables["S"].as<unsigned>(), d); \
            } \
            return 1.0 / (1.0 + static_cast<double>(collisions)); \
        })

namespace po = boost::program_options;
po::variables_map variables{};

bool solution_found = false;

template <typename system_type>
void run_system(system_type&& system)
{
    unsigned generation;
    double fitness_mean, fitness_std_dev;
    typename system_type::individual_type const* best_individual;

    const auto early_stop = variables.count("stop") > 0;

    while (true)
    {
        std::tie(generation, fitness_mean, fitness_std_dev, best_individual) = system.stats();

        std::printf("%d %f %f %f %s\n",
                    generation,
                    best_individual->fitness,
                    fitness_mean,
                    fitness_std_dev,
                    boost::lexical_cast<std::string>(best_individual->genotype).c_str());

        if (early_stop and best_individual->fitness == 1.0 or generation >= variables["generations"].as<unsigned>())
        {
            break;
        }

        system.evolve();
    }
}

std::vector<bool> tags{};

unsigned evaluate_surprising_sequence_collisions(
    const std::vector<unsigned>& sentence, const unsigned s, const unsigned d)
{
    tags.resize(s * s);
    std::fill(tags.begin(), tags.end(), false);

    unsigned collisions = 0;

    for (std::size_t i = 0; i != sentence.size() - d - 1; ++i)
    {
        const auto offset = s * sentence[i] + sentence[i + d + 1];
        if (tags[offset])
        {
            ++collisions;
        }
        else
        {
            tags[offset] = true;
        }
    }

    return collisions;
}

int main(int argc, char** argv)
{
    try
    {
        po::options_description description{"Options"};
        description.add_options()
            ("L", po::value<unsigned>()->default_value(40), "Surprising string length")
            ("S", po::value<unsigned>()->default_value(40), "Surprising string symbol set size")
            ("adult_selection", po::value<std::string>()->default_value("full"), "Adult selection (full/mixed/over)")
            ("child_count", po::value<unsigned>()->default_value(150), "Child count used in mixed/over adult selection")
            ("crossover_points", po::value<unsigned>()->default_value(1), "Crossover points")
            ("crossover_rate", po::value<double>()->default_value(1.0), "Crossover rate")
            ("epsilon", po::value<double>()->default_value(0.1), "Tournament probability of selecting random winner")
            ("generations", po::value<unsigned>()->default_value(1000), "Generation count")
            ("global", "Global surprising sequence")
            ("group_size", po::value<unsigned>()->default_value(10), "Tournament group size")
            ("local", "Local surprising sequence")
            ("mutation_rate", po::value<double>()->default_value(0.001), "Mutation rate")
            ("parent_selection", po::value<std::string>()->default_value("proportionate"), "Parent selection (proportionate/rank/sigma/tournament)")
            ("population_size", po::value<unsigned>()->default_value(100), "Population size")
            ("rank_max", po::value<double>()->default_value(1.5), "Rank selection pressure ('max')")
            ("stop", "Stop on first encountered solution");

        po::store(po::parse_command_line(argc, argv, description), variables);

        if (variables.count("global") == 0 and variables.count("local") == 0)
        {
            std::cerr << "error: use --global or --local switch" << std::endl;
            return EXIT_FAILURE;
        }

        const auto adult_selection  = variables["adult_selection"].as<std::string>();
        const auto parent_selection = variables["parent_selection"].as<std::string>();

        if (adult_selection == "full" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "full" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(parent_selection, vi::ea::adult_selection::full_generational_replacement{}));
        }
        else if (adult_selection == "mixed" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "mixed" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(
                parent_selection,
                vi::ea::adult_selection::generational_mixing{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "proportionate")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::fitness_proportionate{},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "rank")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::rank{variables["rank_max"].as<double>()},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "sigma")
        {
            run_system(BUILD_SYSTEM(
                vi::ea::parent_selection::sigma{},
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
        else if (adult_selection == "over" and parent_selection == "tournament")
        {
            auto parent_selection = vi::ea::parent_selection::tournament{
                variables["group_size"].as<unsigned>(),
                variables["epsilon"].as<double>()};

            run_system(BUILD_SYSTEM(
                parent_selection,
                vi::ea::adult_selection::overproduction{variables["child_count"].as<unsigned>()}));
        }
    }
    catch (const po::error& error)
    {
        std::cerr << "error: " << error.what() << std::endl;
        return EXIT_FAILURE;
    }

    return solution_found ? 1 : 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Projection example 1, direct

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/extensions/algorithms/parse.hpp>

#include <boost/geometry/extensions/gis/latlong/latlong.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>

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

    // Initialize projection parameters
    projections::parameters par = projections::init("+ellps=WGS84 +units=m");

    // Construct a Robinson projection, using specified point types
    // (This delivers a projection without virtual methods. Note that in p02 example
    //  the projection is created using a factory, which delivers a projection with virtual methods)
    typedef model::ll::point<degree> point_ll_deg;
    typedef model::d2::point_xy<double> point_xy;
    projections::robin_spheroid<point_ll_deg, point_xy> prj(par);

    // Define Amsterdam / Barcelona in decimal degrees / degrees/minutes
    point_ll_deg amsterdam = parse<point_ll_deg>("52.4N", "5.9E");
    point_ll_deg barcelona = parse<point_ll_deg>("41 23'N", "2 11'E");

    point_xy pa, pb;

    // Now do the projection. "Forward" means from latlong to meters.
    // (Note that a map projection might fail. This is not 'exceptional'.
    // Therefore the forward function does not throw but returns false)
    if (prj.forward(amsterdam, pa) && prj.forward(barcelona, pb))
    {
        std::cout << "Amsterdam: " << wkt(pa) << std::endl << "Barcelona: " << wkt(pb) << std::endl;

        std::cout << "Distance (unprojected):" << distance(amsterdam, barcelona) / 1000.0 << " km" << std::endl;
        std::cout << "Distance (  projected):" << distance(pa, pb) / 1000.0 << " km" << std::endl;

        // Do the inverse projection. "Inverse" means from meters to latlong
        // It also might fail or might not exist, not all projections
        // have their inverse implemented
        point_ll_deg a1;
        if (prj.inverse(pa, a1))
        {
            std::cout << "Amsterdam (original): " << wkt(amsterdam)  << std::endl
                << "Amsterdam (projected, and back):" << wkt(a1) << std::endl;
            std::cout << "Distance a-a': " << distance(amsterdam, a1) << " meter" << std::endl;
        }
    }

    return 0;
}


#ifndef TCM_DIELECTRIC_FUNCTION_HPP
#define TCM_DIELECTRIC_FUNCTION_HPP

#include <cmath>
#include <cassert>

#include <iostream>
#include <vector>
#include <array>
#include <fstream>
#include <regex>
#include <algorithm>

#include <boost/core/demangle.hpp>

#include <benchmark.hpp>
#include <logging.hpp>

#include <constants.hpp>
#include <matrix.hpp>
#include <blas.hpp>

namespace tcm {

template <class T> struct huge_val;

template <>
struct huge_val<float> { static constexpr float value = HUGE_VALF; };

template <>
struct huge_val<double> { static constexpr double value = HUGE_VAL; };

template <>
struct huge_val<long double> { static constexpr long double value = HUGE_VALL; };

constexpr float huge_val<float>::value;

///////////////////////////////////////////////////////////////////////////////
/// \brief Computes \f$ f(E) \f$

/// Calculates the Fermi-Dirac distribution:
/// \f[ f(E) := \frac{1}{\exp(\frac{E - \mu}{k_{\text{B}}T}) + 1}. \f]
///
/// In the calculation we use that p
/// \f[ \begin{aligned}
///         \lim_{E\to -\infty} f(E) &= 1 //
///         \lim_{E\to +\infty} f(E) &= 0
///     \end{algned}
/// \f]
/// If `std::exp` returns HUGE_VAL, HUGE_VALF or HUGE_VALL, 0 is returned.
/// \param     E       Energy, i.e. a real number. Thus `_F` must be floating
///                    point: `std::is_floating_point<_F>::value == true`.
/// \param     mu      Chemical potential \f$ \mu \f$. It has the dimensions
///                    of energy \f$\implies\f$ `_R` is also floating point.
/// \param     t       Temperature \f$ T \f$.
/// \param     kb      Boltzmann constant \f$ k_\text{B} \f$.
///
/// \return \f$ f(E) \f$.
///////////////////////////////////////////////////////////////////////////////
template<class _F, class _R>
auto fermi_dirac(_F const E, _R const t, _R const mu, _R const kb ) noexcept
{
	static_assert(std::is_floating_point<_F>::value, "Energy must be real.");
	static_assert(std::is_floating_point<_R>::value, "Chemical potential, "
		"temperature and Boltzmann's constant must be real.");

	auto const x = std::exp((E - mu) / (kb * t));

	if (std::isinf(x)) return 0.0;
	if (x < std::numeric_limits<decltype(x)>::epsilon()) return 1.0;
	return 1.0 / (x + 1.0);
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Defines tools to calculate the \f$ G(\omega) \f$ matrix.
///////////////////////////////////////////////////////////////////////////////
namespace g_function {

///////////////////////////////////////////////////////////////////////////////
/// \brief Computes \f$G_{i, j}(\omega)=\frac{f_i-f_j}{E_i-E_j-\omega}\f$.

/// \tparam    _F      Represents a real/complex field.
/// \tparam    _R      Represents a real field.
/// \tparam    _Number Just some abstract field.
/// \param     i       Row of the matrix \f$G(\omega)\f$.
/// \param     j       Column of the matrix \f$G(\omega)\f$.
/// \param     E       Pointer into the array of energies. `_F` must be real,
///                    and size of \p E must be at least `max(i,j)`.
/// \param     f       Pointer into the array of occupational numbers. `_R`
///                    must be real, and size of \p f must be at least
///                    `max(i,j)`.
///
/// \return \f$G_{i,j}(\omega)\f$.
///////////////////////////////////////////////////////////////////////////////
template<class _F, class _R, class _Number>
auto at( std::size_t const i, std::size_t const j
       , _Number const omega
       , _F const* E
       , _R const* f ) noexcept
{
	static_assert(std::is_floating_point<_F>::value, "Energy must be real.");
	static_assert(std::is_floating_point<_R>::value, "Occupational numbers "
		"must be real.");
	return (f[i] - f[j]) / (E[i] - E[j] - omega);
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Computes \f$ G(\omega) \f$.

/// \f[ G_{i,j}(\omega) = \frac{f_i - f_j}{E_i - E_j - \omega}. \f]
///
/// \param omega    Frequency \f$\omega\f$ at which to calculate \f$ G \f$.
///                 It may be either real or complex.
/// \param E        Energies of the system: \f$1 \times N\f$ matrix (i.e. a
///                 column vector). `_F` must be floating point.
/// \param cs       Constants map. This function requires the availability of
///                 \f$ T, \mu, k_\text{B}, \hbar \f$ to run. Checks are
///                 performed at runtime, i.e. this function <b>may throw</b>!
///                 `_R`, the type of constants in \p cs must also be floating
///                 point.
/// \param lg       The logger.
/// \return         \f$G(\omega)\f$.
/// \exception      May throw.
///////////////////////////////////////////////////////////////////////////////
template<class _Number, class _F, class _R, class _Logger>
auto make( _Number const omega
         , Matrix<_F> const& E
         , std::map<std::string, _R> const& cs
         , _Logger & lg )
{
	static_assert(std::is_floating_point<_F>::value, "Energy must be real.");
	static_assert(std::is_floating_point<_R>::value, "Physical constants "
		"such as chemical potential and temperature must be real.");
	using Real = decltype( fermi_dirac( std::declval<_F>()
	                                  , std::declval<_R>()
	                                  , std::declval<_R>()
	                                  , std::declval<_R>() ));
	using Complex = decltype( at( std::declval<std::size_t>()
	                            , std::declval<std::size_t>()
	                            , std::declval<_Number>()
	                            , std::declval<_F const*>()
	                            , std::declval<_R const*>() ));

	TCM_MEASURE( "g_function::make<" + boost::core::demangle(
		typeid(Complex).name()) + ">()" );
	LOG(lg, debug) << "Calculating G for omega = " << omega << "...";
	require(__PRETTY_FUNCTION__, cs, "temperature");
	require(__PRETTY_FUNCTION__, cs, "chemical-potential");
	require(__PRETTY_FUNCTION__, cs, "boltzmann-constant");
	assert( is_column(E) == 1 );
	const auto t  = cs.at("temperature");
	const auto mu = cs.at("chemical-potential");
	const auto kb = cs.at("boltzmann-constant");
	const auto N  = E.height();

	Matrix<Real> f{N, 1};
	std::transform( E.data(), E.data() + N, f.data()
	              , [t, mu, kb](auto Ei) noexcept
	                { return fermi_dirac(Ei, t, mu, kb); } );
	Matrix<Complex> G{N, N};
	for (std::size_t j = 0; j < N; ++j) {
		for (std::size_t i = 0; i < N; ++i) {
			G(i,j) = at(i, j, omega, E.data(), f.data());
		}
	}

	LOG(lg, debug) << "Successfully calculated G.";
	return G;
}

} // namespace g_function

///////////////////////////////////////////////////////////////////////////////
/// Defines tools to compute \f$ \chi(\omega) \f$ matrix.
///////////////////////////////////////////////////////////////////////////////
namespace chi_function {

///////////////////////////////////////////////////////////////////////////////
/// \brief Calculates \f$ \chi_{a,b}(\omega) \f$.

/// We compute \f$ \chi_{a,b}(\omega) \f$ as
/// \f[ \begin{aligned}
///         A &:= \psi_a \circ \psi_b^*,
///         \text{ i.e. } A_i = \psi_{i,a}\cdot\psi{i,b}^* \\ \text{}
///         \chi_{a,b}(\omega) &= A^\dagger \times G(\omega) \times A.
///     \end{aligned}
/// \f]
///
/// Hadamard product \f$ \circ \f$ is computed using `std::transform`, which
/// can easily be parallelized using GNU parallel mode of the `stdlibc++`.
/// Matrix-vector products are calculated by first calling `?GEMV` and then
/// `?DOTC`. Intel MKL has a highly parallel implementation of both of this
/// functions.
///
/// \param a    Row of the matrix.
/// \param b    Column of the matrix.
/// \param Psi  Eigenstates of the system.
/// \param G    G function calculated by calling g_function::make().
///
/// \returns    \f$ \chi_{a,b}(\omega) \f$.
/// \exception  May throw if memory allocations or LAPACK operations fail.
///////////////////////////////////////////////////////////////////////////////
template <class _F, class _C>
auto at( std::size_t const a, std::size_t const b
       , Matrix<_F> const& Psi, Matrix<_C> const& G )
{
	TCM_MEASURE( "chi_function::at<" + boost::core::demangle(
		typeid(_F).name()) + ", " + boost::core::demangle(
		typeid(_C).name()) + ">()" );
	using Complex = std::common_type_t<_F, _C>;

	const auto N = Psi.height();
	Matrix<Complex>    A{N, 1};
	Matrix<Complex> temp{N, 1};

	std::transform( Psi.cbegin_row(a), Psi.cend_row(a)
	              , Psi.cbegin_row(b)
	              , A.data()
	              , [](auto x, auto y) { return x * std::conj(y); } );
	blas::gemv( blas::Operator::T
	          , Complex{1}, G, A
	          , Complex{0}, temp );
	return Complex{2} * blas::dot(A, temp);
}

namespace {
// For the case that eigenstates are actually real.
template<class _Number, class _F, class _R, class _Logger>
auto make_impl( _Number const omega
              , Matrix<_F> const& E
              , Matrix<_F> const& Psi
              , std::map<std::string, _R> const& cs
              , _Logger & lg )
{
	static_assert(std::is_floating_point<_F>::value, "Energy must be real.");
	static_assert(std::is_floating_point<_R>::value, "Physical constants "
		"such as chemical potential and temperature must be real.");
	TCM_MEASURE( "chi_function::make_impl<" + boost::core::demangle(
		typeid(_F).name()) + ">()" );
	auto const N = E.height();
	auto const G = g_function::make(omega, E, cs, lg);

	using T = decltype( at( std::declval<std::size_t>()
	                      , std::declval<std::size_t>()
	                      , std::declval<Matrix<_F>>()
	                      , std::declval<decltype(G)>() ));
	Matrix<T> Chi{N, N};

	auto const _total_points_ = static_cast<double>(N * (N - 1) / 2);
	auto _start_ = std::chrono::system_clock::now();
	// fill the diagonal
	for (std::size_t i = 0; i < N; ++i) {
			if ((_start_ - std::chrono::system_clock::now()) >
				std::chrono::minutes{5}) {

				LOG(lg, info) << "at " << std::round((i + 1) / _total_points_)
				              << "% ..." << std::flush;
				_start_ = std::chrono::system_clock::now();
			}
			Chi(i, i) = at(i, i, Psi, G);
	}
	// calculate upper triangle
	for (std::size_t j = 0; j < N; ++j) {
		for (std::size_t i = 0; i < j; ++i) {
			if ((_start_ - std::chrono::system_clock::now()) >
				std::chrono::minutes{5}) {

				LOG(lg, info) << "at " << std::round((N + (i + 1)* (j + 1))
					/ _total_points_)
				              << "% ..." << std::flush;
				_start_ = std::chrono::system_clock::now();
			}
			Chi(i, j) = at(i, j, Psi, G);
			Chi(j, i) = Chi(i, j);
		}
	}
	return Chi;
}

// For the case that eigenstates are complex.
template<class _Number, class _F, class _R, class _Logger>
auto make_impl( _Number const omega
              , Matrix<_F> const& E
              , Matrix<std::complex<_F>> const& Psi
              , std::map<std::string, _R> const& cs
              , _Logger & lg )
{
	static_assert(std::is_floating_point<_F>::value, "Energy must be real.");
	static_assert(std::is_floating_point<_R>::value, "Physical constants "
		"such as chemical potential and temperature must be real.");
	TCM_MEASURE( "chi_function::make_impl<" + boost::core::demangle(
		typeid(std::complex<_F>).name()) + ">()" );
	auto const N = E.height();
	auto const G = g_function::make(omega, E, cs, lg);

	using T = decltype( at( std::declval<std::size_t>()
	                      , std::declval<std::size_t>()
	                      , std::declval<Matrix<std::complex<_F>>>()
	                      , std::declval<decltype(G)>() ));
	Matrix<T> Chi{N, N};

	auto const _total_points_ = static_cast<double>(N * N);
	auto _start_ = std::chrono::system_clock::now();
	for (std::size_t j = 0; j < N; ++j) {
		for (std::size_t i = 0; i < N; ++i) {
			if ((_start_ - std::chrono::system_clock::now()) >
				std::chrono::minutes{5}) {

				LOG(lg, info) << "at " << std::round(((i + 1) * (j + 1)) / _total_points_)
				              << "% ..." << std::flush;
				_start_ = std::chrono::system_clock::now();
			}
			Chi(i, j) = at(i, j, Psi, G);
		}
	}
	return Chi;
}
} // end unnamed namespace

///////////////////////////////////////////////////////////////////////////////
/// \brief Calculates \f$ \chi(\omega) \f$.

/// First, calculates \f$ G(\omega) \f$ by calling tcm::g_function::make and
/// then computes each matrix element by calling at().
/// \tparam _Number   Some abstract field.
/// \tparam _F        Complex or real field.
/// \tparam _C        Complex field.
/// \tparam _F        Real field.
/// \tparam _Logger   Type of the logger.
/// \param omega      Frequency \f$ \omega \f$ at which to calculate
///                   \f$\chi\f$.
/// \param E          Eigenenergies of the system.
/// \param Psi        Eigenstates of the system.
/// \param cs         Constants.
/// \param lg         Logger object.
///
/// \returns \f$ \chi(\omega) \f$ as a Matrix<_C>.
/// \exception May throw.
///////////////////////////////////////////////////////////////////////////////
template<class _Number, class _F, class _C, class _R, class _Logger>
auto make( _Number const omega
         , Matrix<_F> const& E
         , Matrix<_C> const& Psi
         , std::map<std::string, _R> const& cs
         , _Logger & lg )
{
	TCM_MEASURE( "chi_function::make<" + boost::core::demangle(
		typeid(_C).name()) + ">()" );
	LOG(lg, debug) << "Calculating chi for omega = " << omega << "...";

	const auto N = E.height();
	assert( is_column(E) );
	assert( is_square(Psi) );
	assert( N == Psi.height() );

	auto const Chi = make_impl(omega, E, Psi, cs, lg);
	LOG(lg, debug) << "Successfully calculating chi.";
	return Chi;
}

} // namespace chi_function

///////////////////////////////////////////////////////////////////////////////
/// \brief Defines tools to calculate the Coulomb interaction potential.
///////////////////////////////////////////////////////////////////////////////
namespace coulomb {

namespace {
///////////////////////////////////////////////////////////////////////////////
/// \brief Computes the distance between two points in \f$ F^3 \f$.
///////////////////////////////////////////////////////////////////////////////
template<class _F>
auto distance(std::array<_F, 3> const& v, std::array<_F, 3> const& w)
{
	return std::sqrt( std::pow(std::abs(v[0] - w[0]), 2)
	                + std::pow(std::abs(v[1] - w[1]), 2)
	                + std::pow(std::abs(v[2] - w[2]), 2)
	                );
}
} // unnamed namespace

///////////////////////////////////////////////////////////////////////////////
/// \brief Calculates \f$ V_{i,j} \f$.

/// Potential is calculated as following
/// \f[ V_{i,j} = \left\{
///     \begin{aligned}
///         &\frac{e}{4\pi\varepsilon_0|\mathbf{r_i} - \mathbf{r_j}|}
///              , &\text{if } i \neq j, \\ \text{}
///         &V_0, &\text{if } i = j.
///     \end{aligned}
///     \right.
/// \f]
///
/// \tparam _F        Real field.
/// \tparam _R        Another real field.
/// \param i          Index of the first atom, i.e. row of the matrix.
/// \param j          Index of the second atom,.i.e. column of the matrix.
/// \param positions  Positions of the atoms, in __meters__.
/// \param e          Elementary charge, in Coulombs.
/// \param pi         \f$\pi\f$.
/// \param eps0       \f$\varepsilon_0\f$.
/// \param v0         Self-interaction potential \f$V_0\f$, in eV.
///
/// \returns Potential in eV.
/// \exceptions Should not throw.
///////////////////////////////////////////////////////////////////////////////
template<class _F, class _R>
auto at( std::size_t const i, std::size_t const j
       , std::vector<std::array<_F, 3>> const& positions
       , _R const e
       , _R const pi
       , _R const eps0
       , _R const v0 ) noexcept
{
	return (i == j)
		? v0
		: e / (_R{4.0} * pi * eps0 * distance(positions[i], positions[j]));
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Construct an Elemental-like matrix \f$ V \f$, representing the
/// Coulomb potential.

/// \tparam _T       Element type of the output Matrix.
/// \tparam _F       Real field.
/// \tparam _R       Another real field.
/// \tparam _Logger  The logger type.
/// \param positions Array with positions of atoms.
/// \param cs        Constants. `elementary-charge`, `vacuum-permittivity`,
///                  `pi` and `self-interaction-potential` are needed.
/// \param lg        The logger.
///
/// \return Matrix describing the Coulomb interaction.
/// \exception May throw.
///////////////////////////////////////////////////////////////////////////////
template<class _T, class _F, class _R, class _Logger>
auto make( std::vector<std::array<_F, 3>> const& positions
         , std::map<std::string, _R> const& cs
         , _Logger & lg ) -> Matrix<_T>
{
	TCM_MEASURE( "coulomb::make<" + boost::core::demangle(
		typeid(_T).name()) + ">()" );
	LOG(lg, debug) << "Calculating V...";

	require(__PRETTY_FUNCTION__, cs, "elementary-charge");
	require(__PRETTY_FUNCTION__, cs, "pi");
	require(__PRETTY_FUNCTION__, cs, "vacuum-permittivity");
	require(__PRETTY_FUNCTION__, cs, "self-interaction-potential");

	auto const N         = positions.size();
	auto const e         = cs.at("elementary-charge");
	auto const pi        = cs.at("pi");
	auto const eps0      = cs.at("vacuum-permittivity");
	auto const v0        = cs.at("self-interaction-potential");

	auto const V = build_matrix
		( N, N
		, [&positions, e, pi, eps0, v0] (auto i, auto j)
		  { return boost::numeric_cast<_T>(
		        at(i, j, positions, e, pi, eps0, v0)
			);
		  }
		);

	LOG(lg, debug) << "Successfully calculated V.";
	return V;
}

} // namespace coulomb

///////////////////////////////////////////////////////////////////////////////
/// \brief Defines tools to compute the dielectric function.
///////////////////////////////////////////////////////////////////////////////
namespace dielectric_function {

///////////////////////////////////////////////////////////////////////////////
/// \brief Calculates the dielectric function matrix \f$\epsilon(\omega)\f$.
///////////////////////////////////////////////////////////////////////////////
template< class _Number, class _F, class _C, class _R, class _T, class _Logger>
auto make( _Number const omega
         , Matrix<_F> const& E
         , Matrix<_C> const& Psi
         , Matrix<_T> const& V
         , std::map<std::string, _R> const& cs
         , _Logger & lg )
{
	TCM_MEASURE( "dielectric_function::make<" + boost::core::demangle(
		typeid(_C).name()) + ">()" );
	LOG(lg, debug) << "Calculating epsilon for omega = " << omega << "...";

	const auto N = E.height();
	assert( is_column(E) );
	assert( is_square(Psi) );
	assert( is_square(V) );
	assert( Psi.height() == N );
	assert( V.height() == N );

	auto const Chi = chi_function::make(omega, E, Psi, cs, lg);

	static_assert(std::is_same<_T, typename decltype(Chi)::value_type>::value, "");
	Matrix<_T> epsilon{N, N};
	for (std::size_t j = 0; j < N; ++j) {
		for (std::size_t i = 0; i < N; ++i) {
			epsilon(i, j) = (i == j) ? 1.0 : 0.0;
		}
	}

	blas::gemm( blas::Operator::None, blas::Operator::None
	          , _T{-1.0}, V, Chi
	          , _T{ 1.0}, epsilon );

	LOG(lg, debug) << "Successfully calculating epsilon.";
	return epsilon;
}

} // namespace dielectric function

} // namespace tcm

#endif // TCM_DIELECTRIC_FUNCTION_HPP


#include <fstream>
#include <iostream>
#include <experimental/filesystem>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "../tasks.hh"
#include "../util/split.hh"
#include "../classes/BinnedTypedMatrix.hh"

namespace fs = std::experimental::filesystem;

using namespace EMC;

using namespace boost::numeric;

int EMC::apply(bool forceOverwrite, std::string inputVectorFileName, std::string outputVectorFileName, std::string matrixFileName) {

	if (!forceOverwrite && fs::exists(outputVectorFileName)) {
		std::cout << "Output file already exists, use -f to force overwrite." << std::endl;
		return 1;
	}

	std::ifstream matFile(matrixFileName);
	BinnedTypedMatrix responseMatrix = BinnedTypedMatrix::readFromFile(matFile);
	matFile.close();

	std::vector<double> inputBinCenter;
	std::vector<double> inputBinValue;
	std::ifstream inputVectorFile(inputVectorFileName);
	while (!inputVectorFile.eof()) {
		std::string line;
		std::getline(inputVectorFile, line );

		if (!line.empty()) {
			try {
				auto splitLine = split(split(split(line, ';'), ','), ' ');
				double binCntr = std::stod(splitLine[0]);
				double binValue = std::stod(splitLine[1]);
				inputBinCenter.push_back(binCntr);
				inputBinValue.push_back(binValue);
			} catch(...) { }
		}
	}
	inputVectorFile.close();

	ublas::vector<ValueError> inputVector(responseMatrix.columnCount);
	std::vector<double> inputVectorBinWidth(responseMatrix.columnCount, 0);

	for (unsigned int i = 0; i < inputBinCenter.size() && inputBinCenter[i] < responseMatrix.columnIndex.back() ; i++) {
		int fillPosition = -1;
		while ( (inputBinCenter[i] > responseMatrix.columnIndex.at(fillPosition + 1)) ) {
			fillPosition++;
		}

		//std::cout << inputBinCenter[i] << " --> [" << responseMatrix.columnIndex.at(fillPosition) << ", " << responseMatrix.columnIndex.at(fillPosition + 1) << "]" << std::endl;

		if (fillPosition >= 0) {
			inputVector[fillPosition].value += inputBinValue[i];
			if (i == 0) {
				inputVectorBinWidth[fillPosition] += inputBinCenter[i+1] - inputBinCenter[i];
			} else {
				inputVectorBinWidth[fillPosition] += inputBinCenter[i] - inputBinCenter[i-1];
			}
		}
	}

	for (unsigned int i=0; i<responseMatrix.columnCount; i++) {
		inputVector[i].err_sq = inputVector[i].value;
		inputVector[i].value /= inputVectorBinWidth[i];
		inputVector[i].err_sq /= (inputVectorBinWidth[i]*inputVectorBinWidth[i]);
		std::cout << "inputVectorBinWidth[i] = " << inputVectorBinWidth[i] << std::endl;
	}

	ublas::vector<ValueError> outputVector = ublas::prod(responseMatrix.m, inputVector);

    std::ofstream outputFstep(outputVectorFileName + ".step");

    for (unsigned int i=0; i<responseMatrix.rowCount; i++) {
    	outputFstep << responseMatrix.rowIndex[i]   << " " << outputVector[i].value << std::endl;
    	outputFstep << responseMatrix.rowIndex[i+1] << " " << outputVector[i].value << std::endl;
	}

    outputFstep.close();

    std::ofstream outputFcntr(outputVectorFileName + ".cntr");

    for (unsigned int i=0; i<responseMatrix.rowCount; i++) {
    	outputFcntr << (0.5*(responseMatrix.rowIndex[i] + responseMatrix.rowIndex[i+1]))   << " " << outputVector[i].value << " " << sqrt(outputVector[i].err_sq) << std::endl;
	}

    outputFcntr.close();

    return 0;
}


#pragma once

#include <iostream>

#include <chrono>
#include <functional>
#include <memory>
#include <optional>

#include <boost/hana.hpp>

#include "SignalPt.hpp"

namespace chrono = std::chrono;
using namespace std::chrono_literals;

template <typename Clock = chrono::steady_clock>
struct PIDState {
  chrono::time_point<Clock> time;
  double errSum;
  double error;
  double ctrlVal;
};

template <typename Clock = chrono::steady_clock>
auto pid_algebra(double kp, double ki, double kd) {
  return [kp, ki, kd](PIDState<Clock> prev,
                      SignalPt<double, Clock> errSigl) -> PIDState<Clock> {
    const chrono::duration<double> deltaT =
        errSigl.time - prev.time;
    if (deltaT <= chrono::seconds{0}) return prev;
    const auto errSum = prev.errSum + (errSigl.value * deltaT.count());
    const auto dErr = (errSigl.value - prev.error) / deltaT.count();
    const auto ctrlVal = kp * errSigl.value + ki * errSum + kd * dErr;

    return {errSigl.time, errSum, errSigl.value, ctrlVal};
  };
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2013 John Maddock
//  Copyright 2013 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _BOOST_BERNOULLI_2013_05_30_HPP_
  #define _BOOST_BERNOULLI_2013_05_30_HPP_

  #include <boost/array.hpp>
  #include <boost/cstdint.hpp>
  #include "detail/bernoulli_b2n.hpp"

  namespace boost { namespace math {

  template <class T, class Policy>
  inline T bernoulli_b2n(const int i, const Policy &pol)
  {
    if(i<0)
      policies::raise_domain_error<T>("boost::math::bernoulli<%1%>", "Index should be >= 0 but got %1%", T(i), pol);
    const int i_2 = 2 * i;

    return boost::math::detail::bernoulli_number_imp<T,Policy>(i_2, pol);
  }

  template <class T>
  inline T bernoulli_b2n(const int i)
  {
    return boost::math::bernoulli_b2n<T>(i, policies::policy<>());
  }

  template <class T, class OutputIterator, class Policy>
  inline OutputIterator bernoulli_b2n(int start_index,
                                      unsigned number_of_bernoullis_b2n,
                                      OutputIterator out_it,
                                      const Policy& pol)
  {
    return boost::math::detail::bernoulli_series_imp<T, OutputIterator, Policy>(start_index,
                                                                                number_of_bernoullis_b2n,
                                                                                out_it,
                                                                                pol);
  }

  template <class T, class OutputIterator>
  inline OutputIterator bernoulli_b2n(int start_index,
                                      unsigned number_of_bernoullis_b2n,
                                      OutputIterator out_it)
  {
    return boost::math::bernoulli_b2n<T, OutputIterator>(start_index,
                                                         number_of_bernoullis_b2n,
                                                         out_it,
                                                         policies::policy<>());
  }

} } // namespace boost::math

#endif // _BOOST_BERNOULLI_2013_05_30_HPP_


// Std includes
#include <iostream> // cout, endl
#include <vector>
#include <memory> // shared_ptr
#include <map>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "fl0p/stationary.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;

template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
using TypeMeshSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeFlow = fl0w::fl0p::Stationary<TypeVector, TypeMatrix, TypeRef, TypeMesh, TypeContainer, TypeMeshSub, TypeContainer>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") = \n" << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << ") = \n" << flow.getJacobian(x, t) << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") = \n" << flow.getVorticity(x, t).transpose() << std::endl;
    std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") = \n" << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

double f(const double x, const double y) {
    return x;
}

int main () {
    // parameters
    std::size_t n = 8;
    // mesh
    std::vector<std::size_t> dimensions({n, n, n});
    std::vector<double> lengths({1.0, 1.0, 1.0});
    TypeVector origin({-0.5, -0.5, -0.5});
    // data
    std::vector<std::vector<float>> velocity(3, std::vector<float>(std::pow(n, DIM)));
    for(unsigned int i = 0; i < n; i++) {
        for(unsigned int j = 0; j < n; j++) {
            for(unsigned int k = 0; k < n; k++) {
                velocity[0][i + n*j + n*n*k] = (i + j + k)/3.0f;
                velocity[1][i + n*j + n*n*k] = (i + j + k)/3.0f;
                velocity[2][i + n*j + n*n*k] = (i + j + k)/3.0f;
            }
        }
    }
    // flow
    TypeFlow flow(std::make_shared<TypeMesh>(dimensions, lengths, origin, TypeContainer<bool>(DIM, true)), velocity, 4);
    // print
    print(flow, TypeVector({0.0, 0.0, 0.0}), 0.0);
}


/**
 * @copyright   Copyright (c) 2021, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */
#include "Motor_current_consumption_model.hpp"
#include <cmath>
#include <cstdio>
#include <limits>
#include <cstdint>
#include <boost/math/tools/rational.hpp>

Motor_current_consumption_model::Motor_current_consumption_model(const std::vector<double>
        &current_consumption_model_coeff) {
    m_current_consumption_model_coeff = current_consumption_model_coeff;
    m_model_type = RPM;
    m_c = 0;
    m_r = 1;
    m_last_current = 0;
}

Motor_current_consumption_model::Motor_current_consumption_model(const std::vector<double>
        &current_consumption_model_coeff, const double C, const double R) {
    if (R <= 0) {
        throw ("R must be greater than 0"); // NOLINT
    }

    if (C < 0) {
        throw ("C must be greater than or equal to 0");  // NOLINT
    }

    m_current_consumption_model_coeff = current_consumption_model_coeff;
    m_model_type = TORQUE;
    m_c = C;
    m_r = R;
    m_last_current = 0;
    // TODO(Mike Lyons) throw and exception if ?? is zero
}

double  Motor_current_consumption_model::get_current(const double value) {
    return boost::math::tools::evaluate_polynomial(m_current_consumption_model_coeff.data(), value,
            m_current_consumption_model_coeff.size());
}

double  Motor_current_consumption_model::get_current(const double value, const double timestep) {
    double current = get_current(value);
    current = current - (m_c / m_r) * (current - m_last_current) /
              (timestep);  // for RPM the dynamic response component equals 0
    m_last_current = current;
    return current;
}