#ifndef PCP_ALGORITHM_BILATERAL_FILTER_HPP
#define PCP_ALGORITHM_BILATERAL_FILTER_HPP

/**
 * @file
 * @ingroup algorithm
 */

#include "pcp/common/norm.hpp"
#include "pcp/common/vector3d.hpp"
#include "pcp/kdtree/linked_kdtree.hpp"
#include "pcp/traits/output_iterator_traits.hpp"
#include "pcp/traits/point_map.hpp"

#include <Eigen/Core>
#include <algorithm>
#include <array>
#include <execution>
#include <vector>

namespace pcp {
namespace algorithm {
namespace bilateral {

/**
 * @brief
 * Parameters for the bilateral filtering algorithm
 */
struct params_t
{
    double sigmaf = 1.;  ///< Standard deviation for the support region of spatial weight function f
    double sigmag = 0.1; ///< Standard deviation for the influence weight function g
    std::size_t K = 1u;  ///< Number of iterations of bilateral filtering
};

namespace detail {

template <
    class KdTree,
    class CoordinateMap,
    class PointMap,
    class NormalMap,
    class SpatialWeightFunction,
    class InfluenceWeightFunction,
    class ProjectionFunction,
    class ScalarType>
std::invoke_result_t<PointMap, std::size_t> compute_pi(
    std::size_t const i,
    ScalarType const sigmaf,
    ScalarType const sigmag,
    KdTree const& kdtree,
    CoordinateMap const& coordinate_map,
    PointMap const& point_map,
    NormalMap const& normal_map,
    SpatialWeightFunction const& f,
    InfluenceWeightFunction const& g,
    ProjectionFunction const& projection)
{
    using scalar_type = ScalarType;
    using point_type  = std::invoke_result_t<PointMap, std::size_t>;
    using normal_type = std::invoke_result_t<NormalMap, std::size_t>;

    scalar_type constexpr two  = scalar_type{2.};
    scalar_type constexpr zero = scalar_type{0.};

    auto const s  = point_map(i);
    auto const ci = coordinate_map(i);

    sphere_a<scalar_type> support_region{};
    support_region.position = ci;
    support_region.radius   = two * sigmaf;

    auto const neighbors = kdtree.range_search(support_region);

    scalar_type k = zero;
    point_type sprime{zero, zero, zero};
    for (auto it = neighbors.begin(); it != neighbors.end(); ++it)
    {
        point_type const p           = point_map(*it);
        normal_type const np         = normal_map(*it);
        point_type const s_projected = projection(p, np, s);

        scalar_type const rf = common::norm(s - p);
        scalar_type const rg = common::norm(s_projected - s);

        scalar_type const wf = f(sigmaf, rf);
        scalar_type const wg = g(sigmag, rg);

        scalar_type const w = wf * wg;
        k += w;
        common::basic_vector3d_t<scalar_type> translation{
            w * s_projected.x(),
            w * s_projected.y(),
            w * s_projected.z()};
        sprime = sprime + translation;
    }
    sprime = sprime / k;

    return sprime;
}

template <
    class KdTree,
    class CoordinateMap,
    class PointMap,
    class NormalMap,
    class SpatialWeightFunction,
    class SpatialWeightFunctionDerivative,
    class InfluenceWeightFunction,
    class InfluenceWeightFunctionDerivative,
    class ProjectionFunction,
    class ScalarType>
std::invoke_result_t<NormalMap, std::size_t> compute_ni(
    std::size_t const i,
    ScalarType const sigmaf,
    ScalarType const sigmag,
    KdTree const& kdtree,
    CoordinateMap const& coordinate_map,
    PointMap const& point_map,
    NormalMap const& normal_map,
    SpatialWeightFunction const& f,
    SpatialWeightFunctionDerivative const& df,
    InfluenceWeightFunction const& g,
    InfluenceWeightFunctionDerivative const& dg,
    ProjectionFunction const& projection)
{
    using scalar_type           = ScalarType;
    using point_type            = std::invoke_result_t<PointMap, std::size_t>;
    using normal_type           = std::invoke_result_t<NormalMap, std::size_t>;
    using matrix_3d_type        = Eigen::Matrix<scalar_type, 3, 3>;
    using column_vector_3d_type = Eigen::Matrix<scalar_type, 3, 1>;
    using row_vector_3d_type    = Eigen::Matrix<scalar_type, 1, 3>;

    scalar_type constexpr two  = scalar_type{2.};
    scalar_type constexpr zero = scalar_type{0.};

    point_type const pcp_s = point_map(i);
    column_vector_3d_type const s{pcp_s.x(), pcp_s.y(), pcp_s.z()};
    auto const ci = coordinate_map(i);

    sphere_a<scalar_type> support_region{};
    support_region.position = ci;
    support_region.radius   = two * sigmaf;

    auto const neighbors = kdtree.range_search(support_region);

    /**
     * Jacobian of sum of:
     *
     * projection(s) * f(||s - p||) * g(||projection(s) - s||)
     */
    matrix_3d_type J_pi_f_g;
    J_pi_f_g.setZero();
    /**
     * sum of:
     *
     * projection(s) * f(||s - p||) * g(||projection(s) - s||)
     */
    column_vector_3d_type pi_f_g;
    pi_f_g.setZero();
    /**
     * Gradient of k(s)
     */
    row_vector_3d_type grad_k;
    grad_k.setZero();
    /**
     * k(s)
     */
    scalar_type k = zero;

    for (auto it = neighbors.begin(); it != neighbors.end(); ++it)
    {
        point_type const pcp_p           = point_map(*it);
        normal_type const pcp_np         = normal_map(*it);
        point_type const pcp_s_projected = projection(pcp_p, pcp_np, pcp_s);

        column_vector_3d_type const p{pcp_p.x(), pcp_p.y(), pcp_p.z()};
        column_vector_3d_type const np{pcp_np.nx(), pcp_np.ny(), pcp_np.nz()};
        column_vector_3d_type const s_projected{
            pcp_s_projected.x(),
            pcp_s_projected.y(),
            pcp_s_projected.z()};

        column_vector_3d_type const sp  = s - p;
        column_vector_3d_type const sps = s_projected - s;
        scalar_type const rf            = sp.norm();
        scalar_type const rg            = sps.norm();

        // f(||s - p||)
        scalar_type const wf = f(sigmaf, rf);
        // g(||projection(s) - s||)
        scalar_type const wg = g(sigmag, rg);

        scalar_type const w = wf * wg;

        // k(s) = sum f(||s - p||) * g(||projection(s) - s||)
        k += w;

        // projection(s) * f(||s - p||) * g(||projection(s) - s||)
        column_vector_3d_type translation{
            w * pcp_s_projected.x(),
            w * pcp_s_projected.y(),
            w * pcp_s_projected.z()};
        pi_f_g += translation;

        // derivative df/dr | r=||s-p||
        scalar_type const wdf            = df(sigmaf, rf);
        row_vector_3d_type const sp_unit = sp.normalized().transpose();
        // grad(f) = (s - p) / ||s - p|| * (df/dr | r=||s-p||)
        row_vector_3d_type const grad_f = sp_unit * wdf;

        // Jacobian of projection(s)
        matrix_3d_type Jpi;
        Jpi(0, 0) = 1 - (np.x() * np.x());
        Jpi(1, 1) = 1 - (np.y() * np.y());
        Jpi(2, 2) = 1 - (np.z() * np.z());
        Jpi(0, 1) = np.x() * np.y();
        Jpi(0, 2) = np.x() * np.z();
        Jpi(1, 2) = np.y() * np.z();
        Jpi(1, 0) = Jpi(0, 1);
        Jpi(2, 0) = Jpi(0, 2);
        Jpi(2, 1) = Jpi(1, 2);

        // derivative dg/dr | r = ||projection(s) - s||
        scalar_type const wdg             = dg(sigmag, rg);
        row_vector_3d_type const sps_unit = sps.normalized().transpose();
        /**
         * Let sps_unit = (projection(s) - s) / ||projection(s) - s||
         *
         * grad(g) =
         * (sps_unit * Jacobian(projection(s)) - sps_unit) *
         * (dg/dr | r = ||projection(s) - s||)
         */
        row_vector_3d_type const grad_g = (sps_unit * Jpi - sps_unit) * wdg;

        /**
         * Product rule grad(f(||s - p||) * g(||projection(s) - s||))
         */
        grad_k += (grad_f * wg) + (wf * grad_g);
        /**
         * Product rule grad(projection(s) * f(||s - p||) * g(||projection(s) - s||))
         */
        J_pi_f_g += (Jpi * wf * wg) + (sps * grad_f * wg) + (sps * wf * grad_g);
    }

    /**
     * Quotient rule grad(u/v) = (1/v^2) * (grad(u)*v - u*grad(v))
     */
    scalar_type const ks2_inv = scalar_type{1.} / (k * k);
    matrix_3d_type const J    = ks2_inv * (J_pi_f_g * k - pi_f_g * grad_k);

    normal_type const pcp_ns = normal_map(i);
    column_vector_3d_type const ns{pcp_ns.nx(), pcp_ns.ny(), pcp_ns.nz()};

    /**
     * In the original normal improvement paper, they use the inverse transpose
     * of the jacobian. In our case, we directly use the jacobian
     * of the filter F(s), because it is in line with the intuition
     * of using the local spatial deformation of the field F(s) to
     * adjust normals.
     */
    // ns' = J^(-T) * ns
    // matrix_3d_type const adj       = J.adjoint();
    column_vector_3d_type ns_prime = J * ns;
    ns_prime.normalize();
    return normal_type{ns_prime.x(), ns_prime.y(), ns_prime.z()};
}

} // namespace detail
} // namespace bilateral

/**
 * @ingroup smoothing-algorithm
 * @brief
 * Uses a bilateral filter to smooth an input point cloud.
 *
 * The bilateral filter is defined in the same way as in
 * 'Jones, Thouis R., Fredo Durand, and Matthias Zwicker. "Normal improvement for point rendering."
 * IEEE Computer Graphics and Applications 24.4 (2004): 53-56.'
 *
 * Internally, the filtering needs a temporary copy of the points p(k)
 * to compute p(k+1), the points at the next iteration. At each iteration,
 * a kd-tree must be built over the points p(k) to support optimized
 * range search queries.
 *
 * The transformation F (the bilateral filter) over our points P is done in parallel at each
 * iteration k.
 *
 * @tparam RandomAccessIter Iterator type satisfying Random Access requirements
 * @tparam OutputIter Iterator type dereferenceable to a type satisfying Point concept
 * @tparam PointMap Type satisfying PointMap concept
 * @tparam NormalMap Type satisfying NormalMap concept
 * @param begin Start iterator of input point cloud
 * @param end End iterator of input point cloud
 * @param out_begin Start iterator of output points
 * @param point_map The point map property map
 * @param normal_map The normal map property map
 * @param params The bilateral filter algorithm's parameters
 * @return End iterator of output sequence
 */
template <class RandomAccessIter, class OutputIter, class PointMap, class NormalMap>
OutputIter bilateral_filter_points(
    RandomAccessIter begin,
    RandomAccessIter end,
    OutputIter out_begin,
    PointMap const& point_map,
    NormalMap const& normal_map,
    bilateral::params_t const& params)
{
    using input_element_type = typename std::iterator_traits<RandomAccessIter>::value_type;
    using input_point_type   = std::invoke_result_t<PointMap, input_element_type>;
    using input_normal_type  = std::invoke_result_t<NormalMap, input_element_type>;
    using scalar_type        = typename input_point_type::coordinate_type;
    using output_point_type  = typename xstd::output_iterator_traits<OutputIter>::value_type;
    // using difference_type    = typename std::iterator_traits<RandomAccessIter>::difference_type;

    static_assert(
        traits::is_point_map_v<PointMap, input_element_type>,
        "point_map must satisfy PointMap concept");

    static_assert(
        traits::is_point_v<output_point_type>,
        "OutputIter must be dereferenceable to a type satisfying Point concept");

    std::size_t const N        = static_cast<std::size_t>(std::distance(begin, end));
    scalar_type const sigmaf   = static_cast<scalar_type>(params.sigmaf);
    scalar_type const sigmag   = static_cast<scalar_type>(params.sigmag);
    std::size_t const K        = params.K;
    scalar_type constexpr pi   = static_cast<scalar_type>(3.14159265358979323846);
    scalar_type constexpr zero = scalar_type{0.};

    assert(K > 0u);
    assert(N > 0u);
    assert(sigmaf > zero);
    assert(sigmag > zero);

    std::vector<std::size_t> indices(N);
    std::iota(indices.begin(), indices.end(), 0u);

    std::vector<input_point_type> points(N);
    std::transform(begin, end, points.begin(), [&](input_element_type const& e) {
        return point_map(e);
    });

    std::vector<input_point_type> temporary_points(N);

    std::vector<input_normal_type> normals(N);
    std::transform(begin, end, normals.begin(), [&](input_element_type const& e) {
        return normal_map(e);
    });

    auto const internal_point_map = [&](std::size_t const i) {
        return points[i];
    };
    auto const internal_normal_map = [&](std::size_t const i) {
        return normals[i];
    };

    auto const gaussian = [=](scalar_type const sigma, scalar_type const r) -> scalar_type {
        scalar_type const s2      = sigma * sigma;
        scalar_type const r2      = r * r;
        scalar_type const power   = -r2 / (2 * s2);
        scalar_type constexpr one = scalar_type{1.};
        scalar_type constexpr two = scalar_type{2.};
        scalar_type const coeff   = one / (sigma * std::sqrt(two * pi));
        return coeff * std::exp(power);
    };

    auto const projection = [&](input_point_type const& p,
                                input_normal_type const& np,
                                input_point_type const& s) -> input_point_type {
        auto const sp = p - s;
        common::basic_vector3d_t<scalar_type> const n{np.nx(), np.ny(), np.nz()};
        auto const d = common::inner_product(sp, n);
        return s + d * n;
    };

    using coordinates_type = std::array<scalar_type, 3u>;

    auto const coordinate_map = [&](std::size_t const pe) {
        auto const p = internal_point_map(pe);
        return coordinates_type{p.x(), p.y(), p.z()};
    };

    kdtree::construction_params_t kdtree_params;
    kdtree_params.compute_max_depth     = true;
    kdtree_params.construction          = kdtree::construction_t::nth_element;
    kdtree_params.max_elements_per_leaf = 64u;

    for (std::size_t k = 0u; k < K; ++k)
    {
        basic_linked_kdtree_t<input_element_type, 3u, decltype(coordinate_map)> kdtree{
            indices.begin(),
            indices.end(),
            coordinate_map,
            kdtree_params};

        std::transform(
            std::execution::par,
            indices.begin(),
            indices.end(),
            temporary_points.begin(),
            [&](std::size_t const i) {
                return bilateral::detail::compute_pi(
                    i,
                    sigmaf,
                    sigmag,
                    kdtree,
                    coordinate_map,
                    internal_point_map,
                    internal_normal_map,
                    gaussian,
                    gaussian,
                    projection);
            });

        std::copy(temporary_points.begin(), temporary_points.end(), points.begin());
    }

    return std::copy(points.begin(), points.end(), out_begin);
}

/**
 * @ingroup smoothing-algorithm
 * @brief
 * Deforms the normal field of an input point cloud using the local deformation field of the
 * 3d bilateral filter (in other words, its Jacobian at a point p).
 *
 * The bilateral filter is defined in the same way as in
 * 'Jones, Thouis R., Fredo Durand, and Matthias Zwicker. "Normal improvement for point rendering."
 * IEEE Computer Graphics and Applications 24.4 (2004): 53-56.'
 *
 * This method should be used only for point rendering. It does not smooth input normals as one
 * would expect for surface reconstruction. For normal smoothing, one should rather look at
 * techniques such as EAR (edge aware resampling):
 *
 * 'Huang, Hui, et al. "Edge-aware point set resampling." ACM transactions on graphics (TOG) 32.1
 * (2013): 1-12.'
 *
 * @tparam RandomAccessIter Iterator type satisfying Random Access requirements
 * @tparam OutputIter Iterator type dereferenceable to a type satisfying Normal concept
 * @tparam PointMap Type satisfying PointMap concept
 * @tparam NormalMap Type satisfying NormalMap concept
 * @param begin Start iterator of input point cloud
 * @param end End iterator of input point cloud
 * @param out_begin Start iterator of output points
 * @param point_map The point map property map
 * @param normal_map The normal map property map
 * @param params The bilateral filter algorithm's parameters
 * @return End iterator of output sequence
 */
template <class RandomAccessIter, class OutputIter, class PointMap, class NormalMap>
OutputIter bilateral_filter_normals(
    RandomAccessIter begin,
    RandomAccessIter end,
    OutputIter out_begin,
    PointMap const& point_map,
    NormalMap const& normal_map,
    bilateral::params_t const& params)
{
    using input_element_type = typename std::iterator_traits<RandomAccessIter>::value_type;
    using input_point_type   = std::invoke_result_t<PointMap, input_element_type>;
    using input_normal_type  = std::invoke_result_t<NormalMap, input_element_type>;
    using scalar_type        = typename input_point_type::coordinate_type;
    using output_normal_type = typename xstd::output_iterator_traits<OutputIter>::value_type;
    using difference_type    = typename std::iterator_traits<RandomAccessIter>::difference_type;

    static_assert(
        traits::is_point_map_v<PointMap, input_element_type>,
        "point_map must satisfy PointMap concept");

    std::size_t const N        = static_cast<std::size_t>(std::distance(begin, end));
    scalar_type const sigmaf   = static_cast<scalar_type>(params.sigmaf);
    scalar_type const sigmag   = static_cast<scalar_type>(params.sigmag);
    std::size_t const K        = params.K;
    scalar_type constexpr pi   = static_cast<scalar_type>(3.14159265358979323846);
    scalar_type constexpr zero = scalar_type{0.};

    assert(K > 0u);
    assert(N > 0u);
    assert(sigmaf > zero);
    assert(sigmag > zero);

    std::vector<std::size_t> indices(N);
    std::iota(indices.begin(), indices.end(), 0u);

    std::vector<input_normal_type> normals(N);
    std::transform(begin, end, normals.begin(), [&](input_element_type const& e) {
        return normal_map(e);
    });

    std::vector<input_normal_type> temporary_normals(N);

    auto const internal_point_map = [&](std::size_t const i) -> input_point_type {
        return point_map(*std::next(begin, static_cast<difference_type>(i)));
    };
    auto const internal_normal_map = [&](std::size_t const i) -> input_normal_type {
        return normals[i];
    };

    auto const gaussian = [=](scalar_type const sigma, scalar_type const r) {
        scalar_type const s2      = sigma * sigma;
        scalar_type const r2      = r * r;
        scalar_type const power   = -r2 / (2 * s2);
        scalar_type constexpr one = scalar_type{1.};
        scalar_type constexpr two = scalar_type{2.};
        scalar_type const coeff   = one / (sigma * std::sqrt(two * pi));
        return coeff * std::exp(power);
    };

    auto const dgaussian = [=](scalar_type const sigma, scalar_type const r) {
        scalar_type const s2      = sigma * sigma;
        scalar_type const s3      = sigma * s2;
        scalar_type const r2      = r * r;
        scalar_type const power   = -r2 / (2 * s2);
        scalar_type constexpr two = scalar_type{2.};
        scalar_type const coeff   = -r / (s3 * std::sqrt(two * pi));
        return coeff * std::exp(power);
    };

    auto const projection = [&](input_point_type const& p,
                                input_normal_type const& np,
                                input_point_type const& s) -> input_point_type {
        auto const sp = p - s;
        common::basic_vector3d_t<scalar_type> const n{np.nx(), np.ny(), np.nz()};
        auto const d = common::inner_product(sp, n);
        return s + d * n;
    };

    using coordinates_type = std::array<scalar_type, 3u>;

    auto const coordinate_map = [&](std::size_t const i) {
        auto const p = internal_point_map(i);
        return coordinates_type{p.x(), p.y(), p.z()};
    };

    kdtree::construction_params_t kdtree_params;
    kdtree_params.compute_max_depth     = true;
    kdtree_params.construction          = kdtree::construction_t::nth_element;
    kdtree_params.max_elements_per_leaf = 64u;

    basic_linked_kdtree_t<input_element_type, 3u, decltype(coordinate_map)> kdtree{
        indices.begin(),
        indices.end(),
        coordinate_map,
        kdtree_params};

    for (std::size_t k = 0u; k < K; ++k)
    {
        std::transform(
            std::execution::par,
            indices.begin(),
            indices.end(),
            temporary_normals.begin(),
            [&](std::size_t const i) {
                return bilateral::detail::compute_ni(
                    i,
                    sigmaf,
                    sigmag,
                    kdtree,
                    coordinate_map,
                    internal_point_map,
                    internal_normal_map,
                    gaussian,
                    dgaussian,
                    gaussian,
                    dgaussian,
                    projection);
            });

        std::copy(temporary_normals.begin(), temporary_normals.end(), normals.begin());
    }

    return std::copy(normals.begin(), normals.end(), out_begin);
}

} // namespace algorithm
} // namespace pcp

#endif // PCP_ALGORITHM_BILATERAL_FILTER_HPP


#include <Eigen/Dense>
#include <iostream>
#include "ukf.h"

using Eigen::MatrixXd;

int main() {

  // Create a UKF instance
  UKF ukf;

  /**
   * Programming assignment calls
   */
  MatrixXd Xsig = MatrixXd(5, 11);
  ukf.GenerateSigmaPoints(&Xsig);

  // print result
  std::cout << "Xsig = " << std::endl << Xsig << std::endl;

  return 0;
}


#include <iostream>
#include <boost/filesystem.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "tiny_cnn/tiny_cnn.h"

using namespace tiny_cnn;
using namespace tiny_cnn::activation;
namespace fs = boost::filesystem;

std::string label_strs[14] = {
    "3", "C", "D", "E", "F", "H", "J", "K", "L", "M", "N", "W", "X", "Y"
};

void construct_net(network<sequential>& nn) {
    // connection table [Y.Lecun, 1998 Table.1]
#define O true
#define X false
    static const bool tbl[] = {
        O, X, X, X, O, O, O, X, X, O, O, O, O, X, O, O,
        O, O, X, X, X, O, O, O, X, X, O, O, O, O, X, O,
        O, O, O, X, X, X, O, O, O, X, X, O, X, O, O, O,
        X, O, O, O, X, X, O, O, O, O, X, X, O, X, O, O,
        X, X, O, O, O, X, X, O, O, O, O, X, O, O, X, O,
        X, X, X, O, O, O, X, X, O, O, O, O, X, O, O, O
    };
#undef O
#undef X

    // construct nets
    nn << convolutional_layer<tan_h>(32, 32, 5, 1, 6)  // C1, 1@32x32-in, 6@28x28-out
       << average_pooling_layer<tan_h>(28, 28, 6, 2)   // S2, 6@28x28-in, 6@14x14-out
       << convolutional_layer<tan_h>(14, 14, 5, 6, 16,
            connection_table(tbl, 6, 16))              // C3, 6@14x14-in, 16@10x10-in
       << average_pooling_layer<tan_h>(10, 10, 16, 2)  // S4, 16@10x10-in, 16@5x5-out
       << convolutional_layer<tan_h>(5, 5, 5, 16, 120) // C5, 16@5x5-in, 120@1x1-out
       << fully_connected_layer<tan_h>(120, 14);       // F6, 120-in, 14-out
}

// convert image to vec_t
void convert_image(const std::string& imagefilename,
    double minv,
    double maxv,
    int w,
    int h,
    vec_t& data) {
    auto img = cv::imread(imagefilename, cv::IMREAD_GRAYSCALE);
    if (img.data == nullptr) return; // cannot open, or it's not an image

    cv::Mat_<uint8_t> resized;
    cv::resize(img, resized, cv::Size(w, h));

    // mnist dataset is "white on black", so negate required
    std::transform(resized.begin(), resized.end(), std::back_inserter(data),
        [=](uint8_t c) { return (255 - c) * (maxv - minv) / 255.0 + minv; });
}

void load_dataset(std::vector<label_t> &train_labels,
                  std::vector<vec_t> &train_images,
                  std::vector<label_t> &test_labels,
                  std::vector<vec_t> &test_images)
{
    for (int i = 0; i < 14; ++i){
        std::vector<std::string> images;

        fs::directory_iterator end_iter;
        fs::path path("./training_set/"+label_strs[i]);
        for (fs::directory_iterator iter(path); iter != end_iter; ++iter){
            if (fs::extension(*iter)==".png"){
                images.push_back(iter->path().string());
            }
        }

        //train_set.size() : test_set.size() = 4:1
        int flag = 0;
        std::vector<std::string>::iterator itr = images.begin();
        for (;itr != images.end(); ++itr){
            vec_t data;
            convert_image(*itr, -1.0, 1.0, 32, 32, data);
            if (flag <= 4){
                train_labels.push_back(i);
                train_images.push_back(data);
            }else{
                test_labels.push_back(i);
                test_images.push_back(data);
                flag = 0;
            }
            flag++;
        }
    }
}

int main(int argc, char **argv) {
    // specify loss-function and learning strategy
    network<sequential> nn;
    adagrad optimizer;

    construct_net(nn);

    std::cout << "load models..." << std::endl;

    // load training set and test set.
    std::vector<label_t> train_labels;
    std::vector<label_t> test_labels;
    std::vector<vec_t> train_images;
    std::vector<vec_t> test_images;

    load_dataset(train_labels, train_images, test_labels, test_images);

    std::cout << "start training: "<<train_images.size()<<" examples..."<< std::endl;

    progress_display disp(train_images.size());
    timer t;
    int minibatch_size = 100;
    int num_epochs = 50;

    // optimizer.alpha *= std::sqrt(minibatch_size);

    // create callback
    auto on_enumerate_epoch = [&](){
        std::cout << t.elapsed() << "s elapsed." << std::endl;
        tiny_cnn::result res = nn.test(test_images, test_labels);
        std::cout << res.num_success << "/" << res.num_total << std::endl;
        disp.restart(train_images.size());
        t.restart();
    };

    auto on_enumerate_minibatch = [&](){
        disp += minibatch_size;
    };

    // training
    nn.train<mse>(optimizer, train_images, train_labels, minibatch_size, num_epochs,
             on_enumerate_minibatch, on_enumerate_epoch);

    std::cout << "end training." << std::endl;

    // save networks
    std::ofstream ofs("weibo.cn-nn-weights");
    ofs << nn;
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function returns the hyperbolic secant argument \f$\mathop{\textrm{acosh}}(1/x)\f$

    @par Header <boost/simd/function/asech.hpp>

    @see cosh, acosh, sinh, asinh, atanh, atanh, acoth, acsch

    @par Example:

      @snippet asech.cpp asech

    @par Possible output:

      @snippet asech.txt asech

  **/
  IEEEValue asech(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/asech.hpp>
#include <boost/simd/function/simd/asech.hpp>

#endif


#ifndef ENKF_HPP
#define ENKF_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <vector>
#include <functional>
#include <utility>
#include <cassert>
#include <random>
#include <cmath>
#include <iostream>

namespace fastmath
{
  using Vec = Eigen::VectorXd;
  using Mat = Eigen::MatrixXd;

  template<class State_t, class Obs_t, class ObsOp_t, class... ForecastArgs>
  class EnKF
  {
  public:

    constexpr EnKF() {};

    //filter with one observation
    /* virtual void filter(const Obs_t& y);*/

    virtual Obs_t observe(const State_t& x);

    virtual State_t forecast(State_t& x, ForecastArgs&... args);

    virtual ~EnKF() {};
  };

  //scalar EnKF with linear observations
  template<class... ForecastArgs>
  class VectorEnKF : public EnKF<Vec, Vec, Mat, ForecastArgs...>
  {

 private:

    Mat  m_H;

    Vec m_state;

    Vec m_obs = m_H * m_state;

    std::function<Vec(Vec&, ForecastArgs&...)> m_forecast;

    Vec m_ensemble_mean;

    Mat m_ensemble_covariance;

    Mat m_obs_covariance;

    int m_ensemble_size;

    int m_dimension;

    Mat m_ensemble = Mat::Zero(m_dimension, m_ensemble_size);

    bool m_initialized_ensemble = false;

    Mat m_background_covariance = Mat::Identity(m_dimension, m_dimension);

    Mat m_ensemble_transform = gen_covariance_transform(m_ensemble_covariance);
  public:

    VectorEnKF(const std::function<Vec(Vec&, ForecastArgs&...)>& n_forecast,
			 const Mat& n_H,
			 const Vec& n_initial_state,
			 const Mat& n_observe_err_covariance,
			 const Vec& n_initial_ensemble_mean,
			 const Mat&   n_initial_ensemble_covariance,
			 const int    n_ensemble_size
			 ) : EnKF<Vec, Vec, Mat, ForecastArgs...>(),
			     m_H(n_H),
			     m_state(n_initial_state),
			     m_forecast(n_forecast),
			     m_ensemble_mean(n_initial_ensemble_mean),
			     m_ensemble_covariance(n_initial_ensemble_covariance),
			     m_obs_covariance(n_observe_err_covariance),
			     m_ensemble_size(n_ensemble_size),
			     m_dimension(static_cast<int>(n_initial_state.size()))

    {};

    void set_state(const Vec& n_state) noexcept
    {
      m_state = n_state;
    }

    void set_ensemble_size(const int n_ensemble_size) noexcept
    {
      assert(n_ensemble_size > 0);
      m_ensemble_size = n_ensemble_size;
    }

    Vec state()
    {
      return m_state;
    }

    int ensemble_size()
    {
      return m_ensemble_size;
    }

    Vec ensemble_mean()
    {
      return m_ensemble_mean;
    }

    Mat ensemble_covariance()
    {
      return m_ensemble_covariance;
    }

    Vec observe(const Vec& x){
      return m_H * x;
    }

    Vec forecast(Vec& x, ForecastArgs&... args){
      return m_forecast(x, args...);
    }

  private:

    Mat gen_covariance_transform(const Mat& target_covariance) const
    {
      return target_covariance.ldlt().matrixL();
    }

    Mat gen_obs_perturbations()
    {
      auto rcovtransform = gen_covariance_transform(m_obs_covariance);

      std::random_device rd{};
      std::mt19937 gen{rd()};
      std::normal_distribution<> dis{0.0, 1.0};

      Mat A(m_H.rows(), m_ensemble_size);
      Vec epsilon(m_H.rows());

      for(auto i = 0; i < m_ensemble_size; ++i){
	for(auto e = 0; e < m_H.rows(); ++e){
	  epsilon[e] = dis(gen);
	}
	A.col(i) = epsilon;
      }

      return rcovtransform * A;
    }

    Mat gen_ensemble_perturbations(bool gen_cov_transform=true)
    {
      std::random_device rd{};
      std::mt19937 gen{rd()};
      std::normal_distribution<> dis{0.0, 1.0};

      Mat A(m_dimension,m_ensemble_size);
      Vec epsilon(m_dimension);

      if(gen_cov_transform){
	m_ensemble_transform = gen_covariance_transform(m_ensemble_covariance);
      }

      for(auto i = 0; i < m_ensemble_size; ++i){
	for(auto e = 0; e < m_dimension; ++e){
	  epsilon[e] = dis(gen);
	}
	A.col(i) = epsilon + m_ensemble_mean;
      }

      return  m_ensemble_transform * A;// * A;
    }

  /*std::pair<Vec,Mat> sample_mean_covariance(const std::vector<Vec>& sample)
    {
      Vec mu(sample[0].size());

      for(auto i = 0; i < m_ensemble_size; ++i){
	for(auto i = 0; i < sample[0].size(); ++i){
	  mu[i] += s[i];
	}
      }

      mu /= sample.size();

      Mat cov = Mat::Zero(sample.size(), sample.size());

      for(const auto& s : sample){
	auto delta = s - mu;
	cov.noalias() += delta * delta.transpose();
      }

      cov /= (sample.size() - 1);

      return std::make_pair(mu, cov);
      }*/

    //A is the result of a call to form_A
    Mat kalman_gain_A(const Mat& A)
    {
      auto V = m_H * A;

      auto vvr = V * V.transpose();// + m_obs_covariance;
      return A * V.transpose() * vvr.inverse();
    }

    //copy ensemble here
    Mat form_A(Mat ensemble)
    {
      for(auto i = 0; i < m_ensemble_size; ++i){
	ensemble.col(i) -= m_ensemble_mean;
      }

      return ensemble / std::sqrt(m_ensemble_size - 1);
    }

    Mat kalman_gain_ensemble(const Mat& ensemble)
    {
      auto A = form_A(ensemble);
      return kalman_gain_A(A);
    }

    //B matrix given an ensemble
    Mat ensemble_prior_covariance(const Mat& A)
    {
      return A * A.transpose();
    }

    Vec ensemble_member_update(const Mat& K, const Vec& perturbed_y, const Vec& xi)
    {
      auto err = perturbed_y - m_H * xi;

      return xi + K * err;
    }

    Mat ensemble_update(const Mat& K, const Mat& perturbed_yvals, const Mat& ensemble_xvals)
    {
      Mat updated_xvals(ensemble_xvals.rows(), ensemble_xvals.cols());
      for(auto i = 0; i < m_ensemble_size; ++i){
	updated_xvals.col(i) = ensemble_member_update(K, perturbed_yvals.col(i), ensemble_xvals.col(i));
      }
      return updated_xvals;
    }

    Mat ETKF_ensemble_update_mat(Mat& A)
    {
      auto V = m_H * A;
      auto Id = Mat::Identity(m_ensemble_size);
      auto emat = Id + V.transpose() * m_obs_covariance.inverse() * V;

      Eigen::SelfAdjointEigenSolver<Mat> esolver(emat);

      assert(esolver.info() == Eigen::Success);//, "ETKF eigendecomposition failed.");

      Mat Gamma = esolver.eigenvalues().asDiagonal();/* I + Gamma in the notes*/
      auto Q = esolver.eigenvectors();

      for(auto i = 0; i < Gamma.cols(); ++i){
	Gamma(i,i) = std::sqrt(Gamma(i,i));
      }

      return Q * Gamma * Q.transpose();
    }

    Mat ETKF_posterior_ensemble(Mat& A)
    {
      // A is prior ensemble
      auto X = ETKF_ensemble_update_mat(A);
      auto one = Vec::Ones(m_ensemble_size);
      auto aplus = A * one / m_ensemble_size;
      return A - aplus * one.transpose();
    }

    Mat ensemble_posterior_covariance(const Mat& B, const Mat& K)
    {
      return B - K * m_H * B;
    }

  public:

    void ETKF_filter(Vec& obs, ForecastArgs&... args)
    {
      for(auto i = 0; i < m_ensemble_size; ++i){
	  m_ensemble.col(i) = m_state;
	}

      m_ensemble += gen_ensemble_perturbations();

      m_ensemble = ETKF_posterior_ensemble(m_ensemble);

      m_state = m_ensemble.rowwise().mean();

      m_ensemble_mean = m_state;

      m_ensemble_covariance = m_ensemble * m_ensemble.transpose();
    }

    void filter(Vec& obs, ForecastArgs&... args)
    {
      // generate ensemble
      if(!m_initialized_ensemble){
	for(auto i = 0; i < m_ensemble_size; ++i){
	  m_ensemble.col(i) = m_state;
	}
	//m_initialized_ensemble = true;
      }
      m_ensemble += gen_ensemble_perturbations();
      /* forecast ensemble */
      for(auto i = 0; i < m_ensemble_size; ++i){
	Vec xstate = m_ensemble.col(i);
	m_ensemble.col(i) = m_forecast(xstate, args...);
      }

      m_ensemble_mean = m_ensemble.rowwise().mean();

      auto A = form_A(m_ensemble);
      m_background_covariance = A * A.transpose();

      Mat obs_perturbed = gen_obs_perturbations();

      for(auto i = 0; i < m_ensemble_size; ++i){
	obs_perturbed.col(i) += obs;
      }
      auto K = kalman_gain_A(A);
      A = ensemble_update(K, obs_perturbed / std::sqrt(m_ensemble_size-1), A);

      m_ensemble_covariance = A * A.transpose();

      /* get ensemble back */
      A *= std::sqrt(m_ensemble_size - 1);
      //add prior mean back and re-scale
      m_ensemble_mean = std::sqrt(m_ensemble_size - 1) * (A.rowwise().mean() + m_ensemble_mean);
      m_state = m_ensemble_mean;
    }

  };

}//namespace fastmath
#endif


#include <pybindings.h>
#include <container_pybindings.h>
#include <maps/pointing.h>
#include <G3Map.h>
#include <G3Units.h>

#include <boost/math/constants/constants.hpp>

#include <vector>
#include <math.h>
#include <iostream>
#include <iomanip>

#include <stdlib.h>
#include <time.h>

using namespace boost::math;

const double PI = constants::pi<double>();

#define ASIN asin
#define ATAN2 atan2

//#define CHECK_QUAT_INVERSE

/*
 * Quaternions cannot represent parity flips.  Since celestial coordinates
 * and az-el coordinates by construction have a different parity, we can't use
 * the general alpha delta angle to x-y-z mapping for one of the coordiate
 * systems.
 *
 * For the Euclidean quaternion representation at the pole,
 * the z coordinate = -sin(elevation) = sin(declination)
 */

static quat
project_on_plane(quat plane_normal, quat point)
{
	// Projects the quaternion onto a plane with unit normal plane_normal
	//   The plane is defined as going through the origin
	//   with normal = plane_normal

	quat out_q(point);
	//ensure unit vec
	plane_normal /= sqrt(dot3(plane_normal,plane_normal));
	out_q -= plane_normal * dot3(plane_normal, point);
	return out_q;
}

static bool
sloppy_eq(quat a, quat b, double slop = 1e-6)
{
	// Fuzzy quaternion equality comparison
	return ((fabs(a.R_component_1() - (b.R_component_1())) < slop ) &&
		(fabs(a.R_component_2() - (b.R_component_2())) < slop ) &&
		(fabs(a.R_component_3() - (b.R_component_3())) < slop ) &&
		(fabs(a.R_component_4() - (b.R_component_4())) < slop ));
}

quat
ang_to_quat(double alpha, double delta)
{
	double c_delta = cos(delta / G3Units::rad);
	return quat(0,
		    c_delta * cos(alpha/G3Units::rad),
		    c_delta * sin(alpha/G3Units::rad),
		    sin(delta / G3Units::rad));
}

void
quat_to_ang(quat q, double &alpha, double &delta)
{
	double d = dot3(q,q);
	if (fabs(d - 1.0) > 1e-6){
		q /= sqrt(d);
	}
	delta = ASIN(q.R_component_4()) * G3Units::rad;
	alpha = ATAN2(q.R_component_3(), q.R_component_2())*G3Units::rad;
}

static boost::python::tuple
py_quat_to_ang(quat q)
{
	double a,d;
	quat_to_ang(q, a, d);

	return boost::python::make_tuple(a, d);
}

quat
coord_quat_to_delta_hat(quat q)
{
	// computes the delta hat vector for a given point on the unit sphere
	// specified by q
	//
	// (The delta hat is equal to -alpha hat)

	q /= sqrt(dot3(q,q));
	double st = sqrt(1 - (q.R_component_4()*q.R_component_4()));
	quat u= quat(0,
		     -1 * (q.R_component_2() * q.R_component_4())/st,
		     -1 * (q.R_component_3() * q.R_component_4())/st,
		     st);
	u /= sqrt(dot3(u,u));
	return u;
}

double
get_rot_ang(quat start_q, quat end_q, quat trans)
{
	// delta is the physicist spherical coordinates delta
	// Computes delta hat for the start q applies trans to it
	// and then computes the angle between that and end_q's delta hat.

	quat t = trans * coord_quat_to_delta_hat(start_q) / trans;
	quat t_p = coord_quat_to_delta_hat(end_q);

	t /= sqrt(dot3(t,t));
	t_p /= sqrt(dot3(t_p,t_p));
	double d  = dot3(t,t_p);
	double sf = (dot3(end_q, cross3(t, t_p)) < 0) ? -1 : 1;
	if (d > 1) {
		g3_assert(d < 1.01);
		return 0;
	} else if (d < -1) {
		g3_assert(d > -1.01);
		return PI * G3Units::rad;
	} else {
		return sf * acos(d) * G3Units::rad;
	}
}

quat
get_transform_quat(double as_0, double ds_0, double ae_0, double de_0,
    double as_1, double ds_1, double ae_1, double de_1)
{
	/*
	 * as = alpha start
	 * ds = delta start
	 * ae = alpha end
	 * de = delta end
	 *
	 * The numeral postscripts are for which set of points.
	 *
	 * Computes a rotation that will take: (as_0,ds_0) to (ae_0, de_0) and
	 * (as_1, ds_1) to (ae_1, de_1)
	 *
	 */

	quat asds_0 = ang_to_quat(as_0, ds_0);
	quat asds_1 = ang_to_quat(as_1, ds_1);
	quat aede_0 = ang_to_quat(ae_0, de_0);
	quat aede_1 = ang_to_quat(ae_1, de_1);

	quat tquat = cross3(asds_0, aede_0);
	double mag = sqrt(dot3(tquat, tquat));
	double ang = acos(dot3(asds_0, aede_0));
	tquat *= sin(ang/2.0) / mag;
	tquat += quat(cos(ang/2.0),0,0,0);

	// trans_asds_1 and aede_1 should now be the same up to a rotation
	// around aede_0
	quat trans_asds_1 = tquat * asds_1 / tquat;

	// Project them on to a plane and find the angle between the two vectors
	// using (ae_0, de_0) as the normal since we are rotating around that
	// vector.
	quat p_asds1 = project_on_plane(aede_0, trans_asds_1);
	quat p_aede1 = project_on_plane(aede_0, aede_1);
	p_asds1 /= sqrt(dot3(p_asds1,p_asds1));
	p_aede1 /= sqrt(dot3(p_aede1,p_aede1));

	double rot_ang = acos(dot3(p_asds1, p_aede1));
	double sf = (dot3(aede_0, cross3(p_asds1, p_aede1)) < 0) ? -1 : 1;
	rot_ang *= sf;

	double sin_rot_ang_ov_2 = sin(rot_ang/2.0);
	quat rot_quat = quat(cos(rot_ang/2.0),
			     sin_rot_ang_ov_2 * aede_0.R_component_2(),
			     sin_rot_ang_ov_2 * aede_0.R_component_3(),
			     sin_rot_ang_ov_2 * aede_0.R_component_4());
	quat final_trans = rot_quat * tquat;

	return final_trans;
}

static std::vector<double>
test_trans(double az_0, double el_0, double ra_0, double dec_0,
    double az_1, double el_1, double ra_1, double dec_1,
    double az_t, double el_t)
{
	// computes the transform from the first 4 variables
	// returns that transform to the last 2 variables

	double ra_t, dec_t;
	quat q = get_transform_quat(az_0, -el_0,
				    ra_0, dec_0,
				    az_1, -el_1,
				    ra_1, dec_1);
	quat azel = ang_to_quat(az_t, -el_t);
	quat rad = q * azel / q;
	quat_to_ang(rad, ra_t, dec_t);
	std::vector<double> r(2,0);
	r[0] = ra_t;
	r[1] = dec_t;
	return r;
}

static std::vector<double>
test_gal_trans(double az_0, double el_0, double ra_0, double dec_0,
    double az_1, double el_1, double ra_1, double dec_1,
    double az_t, double el_t)
{
	double l_t, b_t;
	quat q = get_fk5_j2000_to_gal_quat() *get_transform_quat(az_0, -el_0,
								 ra_0, dec_0,
								 az_1, -el_1,
								 ra_1, dec_1);
	quat azel = ang_to_quat(az_t, -el_t);
	quat rad = q * azel / q;
	quat_to_ang(rad, l_t, b_t);
	std::vector<double> r(2,0);
	r[0] = l_t;
	r[1] = b_t;
	return r;
}

static double
test_gal_trans_rot(double ra, double dec)
{
	quat start = ang_to_quat(ra,dec);
	quat trans = get_fk5_j2000_to_gal_quat();

	return get_rot_ang(start, trans*start/trans, trans);
}

quat
offsets_to_quat(double x_offset, double y_offset)
{
	// Rotates the point (1,0,0) by the rotation matrix for the y_offset
	// and then the rotation matrix for the x_offset
	// quat t = (quat(cos(x_offset/(2.0*G3Units::rad)),0,0,sin(x_offset/(2.0*G3Units::rad))) *
	// 	  quat(cos(y_offset/(2.0*G3Units::rad)),0,sin(y_offset/(2.0*G3Units::rad)),0));
	// return t*quat(0,1.0,0,0)/t;
	// The above is exactly equal to:
	return ang_to_quat(x_offset, -y_offset);
}

quat
get_origin_rotator(double alpha, double delta)
{
	// Rotates the point (1,0,0) to the point specified by alpha and
	// delta via a rotation about the y axis and then the z axis
        return (quat(cos(alpha/2.0), 0, 0, sin(alpha/2.0)) *
                quat(cos(delta/2.0), 0, -sin(delta/2.0), 0));
}

static void
print_fk5_j2000_to_gal_quat()
{
	// uhh, so, this code was a super lazy way to get the quaternion
	// that takes fk5 j2000 to galactic j2000
	std::cout << std::setprecision(10) << std::endl;
	std::cout << get_transform_quat(
		0,0, 1.6814025470759737, -1.050488399695429,
		0,-0.7853981633974483, 5.750520098164818, -1.2109809382060603)
	          << std::endl;
}

quat
get_fk5_j2000_to_gal_quat()
{
	// returns the quaternion that rotates fk5j2000 to galactic J2000
	// coordinates
	return quat(0.4889475076,-0.483210684,0.1962537583,0.699229742);
}

static void
create_det_az_el_trans(const G3Timestream &az, const G3Timestream &el,
    G3VectorQuat &trans_quats) // XXX: switch to G3TimestreamQuat?
{
	// Creates the transform that takes (1,0,0) to az, -el
	// for why it's -el see the comment at the top of this document

	g3_assert(az.size() == el.size());
	trans_quats = G3VectorQuat(az.size(), quat(1,0,0,0));
	for (size_t i = 0; i < az.size(); i++)
		trans_quats[i] = get_origin_rotator(az[i], -el[i]);
}

static void
create_lazy_det_ra_dec_trans(const G3Timestream &ra, const G3Timestream &dec,
    G3VectorQuat &trans_quats)
{
	// Creates the transform that takes (1,0,0) to ra,dec
	g3_assert(ra.size() == dec.size());
	trans_quats = G3VectorQuat(ra.size(), quat(1,0,0,0));
	for (size_t i = 0; i < ra.size(); i++)
		trans_quats[i] = get_origin_rotator(ra[i], dec[i]);
}

static void
create_det_ra_dec_trans(const G3Timestream &az_0, const G3Timestream &el_0,
     const G3Timestream &ra_0, const G3Timestream &dec_0,
     const G3Timestream &az_1, const G3Timestream &el_1,
     const G3Timestream &ra_1, const G3Timestream &dec_1,
     G3VectorQuat & trans_quats)
{
	// Computes the transform that takes (1,0,0) to the point (ra_0, dec_0)
	// and properly handles rotation about the (ra_0, dec_0) point with the
	// inclusion of the second set of points.
	//
	// Stores the output in trans_quats.

	g3_assert(az_0.size() == el_0.size());
	g3_assert(az_0.size() == el_1.size());
	g3_assert(az_0.size() == az_1.size());
	g3_assert(az_0.size() == dec_0.size());
	g3_assert(az_0.size() == dec_1.size());
	g3_assert(az_0.size() == ra_0.size());
	g3_assert(az_0.size() == ra_1.size());
	trans_quats = G3VectorQuat(ra_0.size(), quat(1,0,0,0));

	for (size_t i = 0; i < ra_0.size(); i++) {
		trans_quats[i] = get_transform_quat(
		    az_0[i], -el_0[i],
		    ra_0[i], dec_0[i],
		    az_1[i], -el_1[i],
		    ra_1[i], dec_1[i]
		    )*get_origin_rotator(az_0[i], -el_0[i]);
	}
}

static void
convert_ra_dec_trans_to_gal(const G3VectorQuat &radec_trans,
    G3VectorQuat &gal_trans)
{
	// Converts a rotation from (1,0,0) to fk5 j2000 into a rotation that
	// takes (1,0,0) to our galactic (l,b)

	gal_trans = G3VectorQuat(radec_trans.size(), quat(1,0,0,0));
	quat gt = get_fk5_j2000_to_gal_quat();
	for (size_t i = 0; i < radec_trans.size(); i++)
		gal_trans[i] = gt*radec_trans[i];
}

void
get_detector_pointing(double x_offset, double y_offset,
    const G3VectorQuat &trans_quat, MapCoordReference coord_sys,
    std::vector<double> &alpha, std::vector<double> &delta)
{
	// For a detector x/y offset and a boresight position specified by
	// trans_quat with a given coordinate system coord_sys,
	// computes the individual detector pointing coordinates.

	quat det_pos = offsets_to_quat(x_offset, y_offset);
	delta.resize(trans_quat.size());
	alpha.resize(trans_quat.size());

	if ((!std::isfinite(x_offset)) || (!std::isfinite(y_offset))){
		log_debug("Found non-finite (inf or nan) offsets");
		for (size_t i=0; i<alpha.size(); i++){
			alpha[i] = nan("");
			delta[i] = nan("");
		}
		return;
	}

	for (size_t i = 0; i < alpha.size(); i++) {
		//using boost inverse
		//quat q=trans_quat[i]*det_pos/trans_quat[i];

		//uses an inverse that assumes we are on the unit sphere
		const quat & t = trans_quat[i];
		quat q=trans_quat[i]*det_pos * quat( t.R_component_1(),
		    -t.R_component_2(), -t.R_component_3(), -t.R_component_4());

		quat_to_ang(q, alpha[i], delta[i]);

		#ifdef CHECK_QUAT_INVERSE
		double a,d;
		quat u = trans_quat[i]*det_pos/trans_quat[i];
		quat_to_ang(u, a, d);
		if( fabs(a - alpha[i]) > 1e-5 || fabs(d - delta[i]) > 1e-5){
			log_fatal("Failed trans %lf %lf %lf %lf\n", a, alpha[i], d, delta[i]);
		}
		#endif
	}
	if (coord_sys == Local) {
		for (size_t i = 0; i < delta.size(); i++)
			delta[i] *= -1;
	}

}

void
get_detector_rotation(double x_offset, double y_offset,
    const G3VectorQuat &trans_quat, std::vector<double> &rot)
{
	// Computes the polarization angle rotation that occurs under the
	// transform trans_quat and stores it in rot.

	rot = std::vector<double>(trans_quat.size(), 0);
	quat det_pos = offsets_to_quat(x_offset, y_offset);
	for (size_t i = 0; i < rot.size(); i++) {
		quat q = trans_quat[i]*det_pos/trans_quat[i];
		rot[i] = get_rot_ang(det_pos, q, trans_quat[i]);
	}
}

static std::vector<double>
convert_celestial_offsets_to_local_offsets(G3VectorQuat trans_vec,
    double x_offset_celest, double y_offset_celest)
{
	double alpha, delta;
 	double x_offset_local, y_offset_local;

	// written at a time when we only have python bindings for the vector quat
	g3_assert( trans_vec.size() == 1);
	quat trans = trans_vec[0];

	// First we compute the celestial position of boresight.
	quat bs_cel = trans * quat(0,1,0,0) / trans;
	quat_to_ang(bs_cel, alpha, delta); //alpha, delta are celestial boresight

	// Next, with our celestial position offsets we compute the position
	// of our detector in celestial coordinates with the celestial offsets.
	// because the offset is in celestial coordinates we do that with the transform
	// that takes (1,0,0) in celestial coordinates to the boresight  while ignoring
	//boresight rotation

	quat celestial_trans = get_origin_rotator(alpha, delta);

	quat det_cel = celestial_trans * offsets_to_quat(x_offset_celest, y_offset_celest) /
	    celestial_trans;

	// We then transform those positions to local coordinates using the full transform
	quat inv_trans = boost::math::conj(trans);
	quat det_local = inv_trans * det_cel / inv_trans;

	#ifdef CHECK_QUAT_INVERSE
	//just to check I'm not insane can be dropped in the future
	g3_assert(sloppy_eq(inv_trans * bs_cel / inv_trans, quat(0,1,0,0)));
	#endif

	// we read off the position of the detector.
	quat_to_ang(det_local, x_offset_local, y_offset_local);

	// C++11 is magic.
	return {x_offset_local, -y_offset_local};
}

static double
angle_d2(double ra_0, double dec_0, double ra_1, double dec_1)
{
	double delta_ra_abs = fabs(ra_0 - ra_1);
	double delta_dec = dec_0 - dec_1;
	if (delta_ra_abs > PI){
		delta_ra_abs = 2 * PI - delta_ra_abs;
	}
	delta_ra_abs /= cos(dec_0);
	return delta_ra_abs*delta_ra_abs + delta_dec*delta_dec;

}

static G3VectorQuat
get_closest_transform(double ra, double dec,
    const G3VectorDouble &ras, const G3VectorDouble &decs,
    const G3VectorQuat &trans)
{
	// Returns quaternion transformation nearest the requested ra/dec

	double dist = angle_d2(ras[0]/G3Units::rad, decs[0]/G3Units::rad,
			       ra/G3Units::rad, dec/G3Units::rad);
	size_t ind = 0;
	for (size_t i = 0; i < ras.size(); i++) {
		double td = angle_d2(ras[i]/G3Units::rad, decs[i]/G3Units::rad,
				     ra/G3Units::rad, dec/G3Units::rad);
		if (td < dist) {
			dist = td;
			ind = i;
		}
	}
	G3VectorQuat v;
	v.push_back(trans[ind]);
	return v;
}

PYBINDINGS("maps")
{
	using namespace boost::python;

	def("test_trans_", test_trans);
	def("test_gal_trans_", test_gal_trans);
	def("test_gal_trans_rot_", test_gal_trans_rot);
	def("print_fk5_j2000_to_gal_quat_", print_fk5_j2000_to_gal_quat);
	def("c_quat_to_ang_", py_quat_to_ang);
	def("c_ang_to_quat_", ang_to_quat);

        def("get_origin_rotator", get_origin_rotator, (arg("alpha"), arg("delta")),
            "Compute the transformation quaternion that would rotate the "
            "vector (1, 0, 0) to point in the given direction.");
        def("offsets_to_quat", offsets_to_quat, (arg("x"), arg("y")),
            "Returns the vector quaternion (0,1,0,0) rotated by the given "
            "x and y offsets.  Equivalent to ``t * quat(0,1,0,0) / t``, where "
            "``t = get_origin_rotator(x, -y)``");
	def("create_det_az_el_trans", create_det_az_el_trans,
	    "Construct a quaternion vector from timestreams of detector "
	    "azimuth and elevation. Equivalent to ``R_z(az) * R_y(-el)``.");
	def("create_lazy_det_ra_dec_trans", create_lazy_det_ra_dec_trans,
	    "Construct a quaternion vector from timestreams of detector "
	    "RA and declination.  Equivalent to ``R_z(ra) * R_y(dec)``");
	def("create_det_ra_dec_trans", create_det_ra_dec_trans,
	    "Construct a quaternion vector from timestreams of detector "
	    "coordinates.  Computes the transformation from local "
	    "(az_0, el_0) coordinates to celestial (ra_0, dec_0), "
	    "accounting for rotation about the boresight by including "
	    "the second set of points.");
	def("convert_ra_dec_trans_to_gal", convert_ra_dec_trans_to_gal,
	    "Rotate a vector of quaternions from Equatorial to Galactic "
	    "coordinates");
}


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
/// This file contains some auxiliary code used for computing 1D
/// Green's functions and related quantities using piecewise cubic
/// interpolation.

#include <cmath>
#include <boost/math/special_functions/pow.hpp>

namespace alma {
namespace aux_cubic {
/// Auxiliary function used when computing rational integrals.
inline double I1(double x0) {
    return std::log(std::abs((1. - x0) / x0));
}

/// Auxiliary function used when computing rational integrals.
inline double I2(double dd, double ee) {
    double den{std::sqrt(4. * ee - boost::math::pow<2>(dd))};

    return -2. * (std::atan(dd / den) - std::atan((dd + 2. * ee) / den)) / den;
}

/// Auxiliary function used when computing rational integrals.
inline double I3(double dd, double ee) {
    double den = std::sqrt(4. * ee - boost::math::pow<2>(dd));

    return (2. * dd * (std::atan(dd / den) - std::atan((dd + 2. * ee) / den)) +
            den * std::log1p(dd + ee)) /
           (2 * ee * den);
}

/// Objects of this class perform rational integrals of the kind
/// found when approximating bands by piecewise cubic polynomials
/// to obtain Green's functions in 1D.
class Cubic_segment {
private:
    /// Coefficients of the cubic polynomial in the denominator.
    const std::array<double, 4> coeff;

public:
    /// Constructor.
    ///
    /// @param[in] a - value of the energy at the left
    /// @param[in] b - value of the energy at the right
    /// @param[in] ap - derivative of the energy at the left
    /// @param[in] bp - derivative of the energy at the right
    Cubic_segment(double a, double b, double ap, double bp)
        : coeff({{-(2. * a + ap - 2. * b + bp),
                  -(-3. * a - 2. * ap + 3. * b - bp),
                  -ap,
                  -a}}) {
    }
    /// Obtain the values of the four integrals needed for the
    /// calculation of the Green's function at a particular
    /// energy.
    ///
    /// @param[in] energ - value of the energy
    /// @return a vector with the four integrals in this order:
    /// ReG, ReGx, ImG, ImGx
    std::array<double, 4> calc_integrals(double ener) const {
        // Coefficients of the full polynomial in the
        // denominator.
        std::array<double, 4> p(this->coeff);
        p[3] += ener;
        // Get the number of roots based on the value of the
        // discriminant for the third-order equation.
        double a{p[1] / p[0]};
        double b{p[2] / p[0]};
        double c{p[3] / p[0]};
        double a2{boost::math::pow<2>(a)};
        double q{(a2 - 3. * b) / 9.};
        double r{(a * (2. * a2 - 9. * b) + 27. * c) / 54.};
        double r2{boost::math::pow<2>(r)};
        double q3{boost::math::pow<3>(q)};
        std::array<double, 4> nruter({{0., 0., 0., 0.}});

        // And use the formulae specific to each case.
        if (r2 < q3) {
            // Three real roots.
            double th{std::acos(r / std::sqrt(q3))};
            double q12{std::sqrt(q)};
            double x0{-2. * q12 * std::cos(th / 3.) - a / 3.};
            double x1{-2. * q12 * std::cos((th + 2. * constants::pi) / 3.) -
                      a / 3.};
            double x2{-2. * q12 * std::cos((th - 2. * constants::pi) / 3.) -
                      a / 3.};
            double pref{-x0 * x1 * x2 / p[3]};
            double aa{pref / ((x0 - x1) * (x0 - x2))};
            double bb{pref / ((x1 - x0) * (x1 - x2))};
            double cc{pref / ((x2 - x0) * (x2 - x1))};
            nruter[0] = aa * I1(x0) + bb * I1(x1) + cc * I1(x2);
            double aax{x0 * aa};
            double bbx{x1 * bb};
            double ccx{x2 * cc};
            nruter[1] = aax * I1(x0) + bbx * I1(x1) + ccx * I1(x2);

            if ((0. < x0) && (x0 < 1.)) {
                nruter[2] += std::abs(aa);
                nruter[3] += std::abs(aax);
            }

            if ((0. < x1) && (x1 < 1.)) {
                nruter[2] += std::abs(bb);
                nruter[3] += std::abs(bbx);
            }

            if ((0. < x2) && (x2 < 1.)) {
                nruter[2] += std::abs(cc);
                nruter[3] += std::abs(ccx);
            }
            nruter[2] *= constants::pi;
            nruter[3] *= constants::pi;
        }
        else {
            // Only one real root.
            double A{-signum(r) * std::cbrt(std::abs(r) + std::sqrt(r2 - q3))};
            double B{(A == 0. ? 0. : q / A)};
            double x0{A + B - a / 3.};
            double dd{A + B + 2. * a / 3.};
            double ee{boost::math::pow<2>(dd / 2.) +
                      .75 * boost::math::pow<2>(A - B)};
            double pref{-x0 * ee / p[3]};
            double aa{pref / (x0 * (x0 + dd) + ee)};
            double bb{-(x0 + dd) * aa};
            double cc{-aa};
            double aax{x0 * aa};
            double bbx{ee * aa};
            double ccx{-aax};
            nruter[0] = aa * I1(x0) + (bb / ee) * I2(dd / ee, 1. / ee) +
                        (cc / ee) * I3(dd / ee, 1. / ee);
            nruter[1] = aax * I1(x0) + (bbx / ee) * I2(dd / ee, 1. / ee) +
                        (ccx / ee) * I3(dd / ee, 1. / ee);

            if ((0. < x0) && (x0 < 1.)) {
                nruter[2] = constants::pi * std::abs(aa);
                nruter[3] = constants::pi * std::abs(aax);
            }
        }
        return nruter;
    }
};
} // namespace aux_cubic
} // namespace alma


/**
 * @file SparseCholesky.hpp
 * @license License BSD-3-Clause
 * @date 2019-10-06
 * @author Timothy A. Davis
 * @copyright LDL Copyright (c) 2005-2012 by Timothy A. Davis. http://www.suitesparse.com
 *
 * LDL License:
 *
 *    Your use or distribution of LDL or any modified version of
 *    LDL implies that you agree to this License.
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 2.1 of the License, or (at your option) any later version.
 *
 *    This library 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 GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301
 *    USA
 *
 *    Permission is hereby granted to use or copy this program under the
 *    terms of the GNU LGPL, provided that the Copyright, this License,
 *    and the Availability of the original version is retained on all copies.
 *    User documentation of any code that uses this code or any modified
 *    version of this code must cite the Copyright, this License, the
 *    Availability note, and "Used by permission." Permission to modify
 *    the code and to distribute modified code is granted, provided the
 *    Copyright, this License, and the Availability note are retained,
 *    and a notice that the code was modified is included.
 *
 * Availability:
 *
 *    http://www.suitesparse.com
 *
 * Stripped down by Alexander Domahidi, 2012.
 * Modified to c++ code by New York University and Max Planck Gesellschaft, 2017
 *
 */

#pragma once

#include <memory>
#include <Eigen/Sparse>
#include <solver/interface/SolverSetting.hpp>

namespace linalg {

  /**
   * Class to perform an LDL factorization of a matrix
   */
  class SparseCholesky
  {
    public:
	  SparseCholesky(){}
	  ~SparseCholesky(){}

	  void analyzePattern(const Eigen::SparseMatrix<double>& mat, const solver::SolverSetting& stgs);
	  int  factorize(const Eigen::SparseMatrix<double>& mat, const Eigen::Ref<const Eigen::VectorXd>& sign);
	  void solve(const Eigen::Ref<const Eigen::VectorXd>& b, double* x);
	  Eigen::VectorXd& solve(const Eigen::VectorXd& b);

    private:
      inline const solver::SolverSetting& getSetting() const { return *setting_; }

	  int n_;
	  double eps_, delta_;
	  Eigen::VectorXd D_, Y_, X_;
	  Eigen::SparseMatrix<double> L_;
	  Eigen::VectorXi Parent_, Pattern_, Flag_, Lnnz_;
	  std::shared_ptr<const solver::SolverSetting> setting_;

  };

}


/*************************************************************************
	> File Name: lqr_steer_control.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Wed Apr 17 11:48:46 2019
 ************************************************************************/

#include <iostream>
#include <limits>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sys/time.h>
#include <Eigen/Eigen>
#include "cubic_spline.h"
#include "motion_model.h"
#include "cpprobotics_types.h"

#define DT 0.1
#define L 0.5
#define KP 1.0
#define MAX_STEER 45.0/180*M_PI

using namespace cpprobotics;
using Matrix5f = Eigen::Matrix<float, 5, 5>;
using Matrix52f = Eigen::Matrix<float, 5, 2>;
using Matrix25f = Eigen::Matrix<float, 2, 5>;
using RowVector5f = Eigen::Matrix<float, 1, 5>;
using Vector5f = Eigen::Matrix<float, 5, 1>;

cv::Point2i cv_offset(
    float x, float y, int image_width=2000, int image_height=2000){
  cv::Point2i output;
  output.x = int(x * 100) + 300;
  output.y = image_height - int(y * 100) - image_height/2;
  return output;
};

Vec_f calc_speed_profile(Vec_f rx, Vec_f ry, Vec_f ryaw, float target_speed){
    Vec_f speed_profile(ryaw.size(), target_speed);

  float direction = 1.0;
  for(unsigned int i=0; i < ryaw.size()-1; i++){
    float dyaw = std::abs(ryaw[i+1] - ryaw[i]);
    float switch_point = (M_PI/4.0< dyaw) && (dyaw<M_PI/2.0);

    if (switch_point) direction = direction * -1;
    if (direction != 1.0) speed_profile[i]= target_speed * -1;
    else speed_profile[i]= target_speed;

    if (switch_point) speed_profile[i] = 0.0;
  }

  for(int k=0; k< 40; k++){
    *(speed_profile.end()-k) = target_speed / (50 - k);
    if (*(speed_profile.end()-k) <= 1.0 / 3.6){
      *(speed_profile.end()-k) = 1.0 / 3.6;
    }
  }
  return speed_profile;
};

float calc_nearest_index(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, int &ind){
  float mind = std::numeric_limits<float>::max();
  for(unsigned int i=0; i<cx.size(); i++){
    float idx = cx[i] - state.x;
    float idy = cy[i] - state.y;
    float d_e = idx*idx + idy*idy;

    if (d_e<mind){
      mind = d_e;
      ind = i;
    }
  }
  float dxl = cx[ind] - state.x;
  float dyl = cy[ind] - state.y;
  float angle = YAW_P2P(cyaw[ind] - std::atan2(dyl, dxl));
  if (angle < 0) mind = mind * -1;

  return mind;
};

Matrix5f solve_DARE(Matrix5f A, Matrix52f B, Matrix5f Q, Eigen::Matrix2f R){
  Matrix5f X = Q;
  int maxiter = 150;
  float eps = 0.01;

  for(int i=0; i<maxiter; i++){
    Matrix5f Xn = A.transpose()*X*A-A.transpose()*X*B*(R+B.transpose()*X*B).inverse() * B.transpose()*X*A+Q;
    Matrix5f error = Xn - X;
    if (error.cwiseAbs().maxCoeff()<eps){
      return Xn;
    }
    X = Xn;
  }

  return X;
};

Matrix25f dlqr(Matrix5f A, Matrix52f B, Matrix5f Q, Eigen::Matrix2f R){
  Matrix5f X = solve_DARE(A, B ,Q, R);
  Matrix25f K = (B.transpose()*X*B + R).inverse() * (B.transpose()*X*A);
  return K;
};

Vec_f lqr_steering_control(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f sp, float& pe, float& pth_e){
  int ind = 0;
  float e = calc_nearest_index(state, cx, cy, cyaw, ind);

  float k = ck[ind];
  float th_e = YAW_P2P(state.yaw - cyaw[ind]);
  float tv = sp[ind];

  Matrix5f A = Matrix5f::Zero();
  A(0, 0) = 1.0;
  A(0 ,1) = DT;
  A(1 ,2) = state.v;
  A(2 ,2) = 1.0;
  A(2 ,3) = DT;
  A(4 ,4) = 1.0;

  Matrix52f B = Matrix52f::Zero();
  B(3, 0) = state.v/L;
  B(4, 1) = DT;

  Matrix5f Q = Matrix5f::Identity();
  Eigen::Matrix2f R = Eigen::Matrix2f::Identity();

  // gain of lqr
  Matrix25f K = dlqr(A, B, Q, R);

  Vector5f x = Vector5f::Zero();
  x(0) = e;
  x(1) = (e-pe)/DT;
  x(2) = th_e;
  x(3) = (th_e-pth_e)/DT;
  x(4) = state.v - tv;

  Eigen::Vector2f ustar = -K * x;

  float ff = std::atan2((L*k), (double)1.0);
  float fb = YAW_P2P(ustar(0));
  float delta = ff+fb;
  float ai = ustar(1);

  pe = e;
  pth_e = th_e;
  return {ai, delta};
};

void update (State& state, float a, float delta){

  if (delta >= MAX_STEER) delta = MAX_STEER;
  if (delta <= - MAX_STEER) delta = - MAX_STEER;

  state.x = state.x + state.v * std::cos(state.yaw) * DT;
  state.y = state.y + state.v * std::sin(state.yaw) * DT;
  state.yaw = state.yaw + state.v / L * std::tan(delta) * DT;
  state.v = state.v + a * DT;

};

void closed_loop_prediction(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profile, Poi_f goal){
  float T = 500.0;
  float goal_dis = 0.3;
  float stop_speed = 0.05;

  State state(-0.0, -0.0, 0.0, 0.0);

  float time_ = 0.0;
  Vec_f x;
  x.push_back(state.x);
  Vec_f y;
  y.push_back(state.y);
  Vec_f yaw;
  yaw.push_back(state.yaw);
  Vec_f v;
  v.push_back(state.v);
  Vec_f t;
  t.push_back(0.0);

  float e = 0;
  float e_th = 0;

  cv::namedWindow("lqr_full", cv::WINDOW_NORMAL);
  int count = 0;
  Vec_f x_h;
  Vec_f y_h;

  while (T >= time_){
    Vec_f control = lqr_steering_control(state, cx, cy, cyaw, ck, speed_profile, e, e_th);
    // float ai = KP * (speed_profile[ind]-state.v);
    update(state, control[0], control[1]);
    // if (std::abs(state.v) <= stop_speed) ind += 1;

    float dx = state.x - goal[0];
    float dy = state.y - goal[1];
    if (std::sqrt(dx*dx + dy*dy) <= goal_dis) {
      std::cout<<("Goal")<<std::endl;
      break;
    }

    x_h.push_back(state.x);
    y_h.push_back(state.y);

    // visualization
    cv::Mat bg(2000, 3000, CV_8UC3, cv::Scalar(255, 255, 255));
    for(unsigned int i=1; i<cx.size(); i++){
      cv::line(
        bg,
        cv_offset(cx[i-1], cy[i-1], bg.cols, bg.rows),
        cv_offset(cx[i], cy[i], bg.cols, bg.rows),
        cv::Scalar(0, 0, 0),
        10);
    }

    for(unsigned int j=0; j< x_h.size(); j++){
      cv::circle(
        bg,
        cv_offset(x_h[j], y_h[j], bg.cols, bg.rows),
        10, cv::Scalar(0, 0, 255), -1);
    }

    cv::putText(
      bg,
      "Speed: " + std::to_string(state.v*3.6).substr(0, 4) + "km/h",
      cv::Point2i((int)bg.cols*0.5, (int)bg.rows*0.1),
      cv::FONT_HERSHEY_SIMPLEX,
      3,
      cv::Scalar(0, 0, 0),
      10);

    // save image in build/bin/pngs
    struct timeval tp;
    gettimeofday(&tp, NULL);
    long int ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;
    std::string int_count = std::to_string(ms);
    cv::imwrite("./pngs/"+int_count+".png", bg);
    // cv::imshow("lqr_full", bg);
    // cv::waitKey(5);
  }
};

int main(){
  Vec_f wx({0.0, 6.0,  12.5, 10.0, 17.5, 20.0, 25.0});
  Vec_f wy({0.0, -3.0, -5.0,  6.5, 3.0, 0.0, 0.0});

  Spline2D csp_obj(wx, wy);
  Vec_f r_x;
  Vec_f r_y;
  Vec_f ryaw;
  Vec_f rcurvature;
  Vec_f rs;
  for(float i=0; i<csp_obj.s.back(); i+=0.1){
    std::array<float, 2> point_ = csp_obj.calc_postion(i);
    r_x.push_back(point_[0]);
    r_y.push_back(point_[1]);
    ryaw.push_back(csp_obj.calc_yaw(i));
    rcurvature.push_back(csp_obj.calc_curvature(i));
    rs.push_back(i);
  }
  float target_speed = 10.0 / 3.6;
  Vec_f speed_profile = calc_speed_profile(r_x, r_y, ryaw, target_speed);
  closed_loop_prediction(r_x, r_y, ryaw, rcurvature, speed_profile, {{wx.back(), wy.back()}});
}


#ifndef EULER1D_HPP_INCLUDED
#define EULER1D_HPP_INCLUDED

// C includes
#include <cmath>
#include <cstdio>

// C++ includes
#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <iterator>
#include <string>
#include <chrono>

// Boost includes
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

// Local includes
#include "vector_utilities.hpp"

// OUT and IN can be redefined as a filestream
// to enable direct file input/output
// In MinGW already defined in windef.h, so need to undef them first
#ifdef OUT
#undef OUT
#endif // OUT
#define OUT std::cout
#ifdef IN
#undef IN
#endif
#define IN  std::cin
#define ERROUT std::cerr
#define LF  std::endl
#define ABS std::abs

// Problem size and number of velocity dimensions to keep track of,
// to avoid magic numbers in loops
#define PRB_DIM 3
#define VEL_DIM 1

// Array sizes
//  - data is in [2..n+1] with [0], [1], [n+2] and [n+3] as buffers
#define NX      (params.nx+4)
#define NXFIRST (2)
#define NXLAST  (params.nx+2)

// Output modes
#define OUT_MODE_STEP 1
#define OUT_MODE_TIME 2

// Time stepping modes
#define TIME_MODE_CONSTANT 1
#define TIME_MODE_VARIABLE 2

// Time stepping methods
#define STEP_EULER   1
#define STEP_RK3TVD  2
#define STEP_RIEMANN 3

// Spatial integration methods
#define METHOD_CENTRAL_FD 1
#define METHOD_ENO        2

// Boundary conditions
#define BOUNDARY_OPEN        1
#define BOUNDARY_PERIODIC    2

// Special problem types
#define PROBLEM_NORMAL  1
#define PROBLEM_PISTON  2
#define PROBLEM_RIEMANN 3

// Function return values
#define RET_OK              0
#define RET_NO_CHANGE       1
#define RET_UPDATED         2
#define RET_ERR_TIME_UNDERFLOW         -1
#define RET_RIEMANN_FAILED_TO_CONVERGE -2

#define EPS            (1e-20)
#define EPS_EQUAL(a,b) (std::abs((a)-(b))<EPS)
#define EPS_ZERO(x)    (std::abs(x)<EPS)
#define MAX(a,b)       ((a)>(b) ? (a) : (b))

typedef struct {
   // Output categories
   bool natural;
   bool conservation;

   // Skipping parameters
   int    skip_mode;
   int    skip_steps;
   double skip_t;

   // File
   std::string filename;
   FILE *file;
} t_output;

typedef struct {
   // Grid parameters
   int    nx;
   double dx;
   double start_x;

   // Time parameters
   int    time_mode;
   int    steps;
   double cfl_number;
   double dt_min;
   double dt_max;
   double t_max;

   // Simulation parameters
   int boundary;
   int time_stepping;
   int scheme;

   // Physical parameters
   double gamma;

   // Problem-specific parameters
   int problem_type;
   int param_dbl_n;
   int param_int_n;
   t_vector param_dbl;
   int     *param_int;
} t_params;

typedef struct {
   // Conserved variables; density, momentum, magnetic field, energy
   t_vectors U;
   // Primitive variables; velocity and pressure
   t_vectors u;
   t_vector  p;
   double dt;
   double t_current;
} t_data;

// File access - open & close
void openFile(  t_output &output );
void closeFile( t_output  output );

// File access - input from ini
void inputData( const std::string filename,
                t_output &output_grid, t_output &output_non_grid, t_params &params, t_data &data );

// File access - output grid data file
void outputGridData( const t_output &output, const t_params &params, const t_data &data, int step, int index );

// File access - output non-grid data to file
void outputNonGridData( const t_output &output, const t_params &params, const t_data &data, int step, int index );

// Conversion - conservation to natural variables
void toNatural( const t_params &params,
                t_data &data );

// Conversion - natural to conservation variables
void toConservation( const t_params &params,
                     t_data &data );

// Time stepper - Third order optimal TVD Runge-Kutta time stepping method
int rk3tvd( t_vectors U, double &dt, const t_params &params );

// Time stepper - Euler time stepping method
int euler_step( t_vectors U, double &dt, const t_params &params );

// Solver - Riemann solver
int riemann_solver( t_data &data, const t_params &params );

// Scheme - central FD
void central_fd( t_vectors U, t_vectors UL, double &dt_step,
                 const t_params &params );

// Scheme - ENO-Roe with characteristics decomposition
void eno_system_roe( t_vectors U, t_vectors UL, double &dt_step,
                     const t_params &params );

#endif // EULER1D_HPP_INCLUDED


#include <Eigen/Dense>

#include "TestsCatchRequire.h"
#include "../core/MatrixUtils.h"

// Dummy function for testing applyFunction
double addOne(double x) {
    return x+1;
}

TEST_CASE("MatrixUtils initializeRandomWeights", "[MatrixUtils]") {
    Eigen::MatrixXd m = Eigen::Matrix2d::Zero();

    SECTION("Test method initialise randomly") {
        MatrixUtils::initializeRandomWeights(m);
        REQUIRE(m.isZero(0) == false);
    }

    SECTION("Test method initialise no seed") {
        Eigen::MatrixXd m2 = Eigen::Matrix2d::Zero();

        MatrixUtils::initializeRandomWeights(m, false);
        MatrixUtils::initializeRandomWeights(m2, false);

        REQUIRE(m.isZero(0) == false);
        REQUIRE(m2.isZero(0) == false);

        REQUIRE(m == m2);
    }
}


#ifndef MATHTOOLBOX_BFGS_HPP
#define MATHTOOLBOX_BFGS_HPP

#include <Eigen/Core>
#include <functional>

namespace mathtoolbox
{
    namespace optimization
    {
        void RunBfgs(const Eigen::VectorXd&                                        x_init,
                     const std::function<double(const Eigen::VectorXd&)>&          f,
                     const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                     const double                                                  epsilon,
                     const unsigned int                                            max_num_iterations,
                     Eigen::VectorXd&                                              x_star,
                     unsigned int&                                                 num_iterations);
    }
} // namespace mathtoolbox

#endif // MATHTOOLBOX_BFGS_HPP


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#ifdef _MSC_VER
#  pragma warning (disable : 4189) //  'd' : local variable is initialized but not referenced
#endif

#include <iostream>
using std::cout; using std::endl;

#include <stdexcept>
using std::domain_error;

//[policy_ref_snip13

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

namespace myspace
{ // using namespace boost::math::policies; // May be convenient in myspace.

  // Define a policy called my_policy to use.
  using boost::math::policies::policy;

// In this case we want all the distribution accessor functions to compile,
// even if they are mathematically undefined, so
// make the policy assert_undefined.
  using boost::math::policies::assert_undefined;

typedef policy<assert_undefined<false> > my_policy;

// Finally apply this policy to type double.
BOOST_MATH_DECLARE_DISTRIBUTIONS(double, my_policy)
} // namespace myspace

// Now we can use myspace::cauchy etc, which will use policy
// myspace::mypolicy:
//
// This compiles but throws a domain error exception at runtime.
// Caution! If you omit the try'n'catch blocks,
// it will just silently terminate, giving no clues as to why!
// So try'n'catch blocks are very strongly recommended.

void test_cauchy()
{
   try
   {
      double d = mean(myspace::cauchy());  // Cauchy does not have a mean!
   }
   catch(const std::domain_error& e)
   {
      cout << e.what() << endl;
   }
}

//] //[/policy_ref_snip13]

int main()
{
   test_cauchy();
}

/*

Output:

policy_snip_13.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\policy_snip_13.exe
  Error in function boost::math::mean(cauchy<double>&): The Cauchy distribution does not have a mean: the only possible return value is 1.#QNAN.

  */


#include <boost/test/unit_test.hpp>
#include <iostream>
#include <vector>
#include "algorithms/math/primes.hpp"

BOOST_AUTO_TEST_SUITE(TestPrimesGeneration)

BOOST_AUTO_TEST_CASE(naive_invalid)
{
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesNaive<int>(-10));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesNaive<int>(0));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesNaive<int>(1));
}

BOOST_AUTO_TEST_CASE(naive_min)
{
    std::vector<int> expected = {2};
    BOOST_CHECK(expected == Algo::Math::GetPrimesNaive<int>(2));
}

BOOST_AUTO_TEST_CASE(naive)
{
    std::vector<int> expected = {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37,
            41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89, 97};

    BOOST_CHECK(expected == Algo::Math::GetPrimesNaive<int>(100));
}

BOOST_AUTO_TEST_CASE(improved_invalid)
{
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesImproved<int>(-10));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesImproved<int>(0));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesImproved<int>(1));
}

BOOST_AUTO_TEST_CASE(improved_min)
{
    std::vector<int> expected = {2};
    BOOST_CHECK(expected == Algo::Math::GetPrimesImproved<int>(2));
}

BOOST_AUTO_TEST_CASE(improved)
{
    std::vector<int> expected = {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37,
            41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89, 97};

    BOOST_CHECK(expected == Algo::Math::GetPrimesImproved<int>(100));
}

BOOST_AUTO_TEST_CASE(seive_invalid)
{
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesSeive<int>(-10));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesSeive<int>(0));
    BOOST_CHECK(std::vector<int>() == Algo::Math::GetPrimesSeive<int>(1));
}

BOOST_AUTO_TEST_CASE(seive_min)
{
    std::vector<int> expected = {2};
    BOOST_CHECK(expected == Algo::Math::GetPrimesSeive<int>(2));
}

BOOST_AUTO_TEST_CASE(seive)
{
    std::vector<int> expected = {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37,
            41, 43, 47, 53, 59, 61, 67, 71, 73, 79, 83, 89, 97};

    BOOST_CHECK(expected == Algo::Math::GetPrimesSeive<int>(100));
}

BOOST_AUTO_TEST_SUITE_END()


#include "randomGraphDrawing.h"
#include <random>
#include <boost/graph/topological_sort.hpp>
#include <boost/range/adaptor/reversed.hpp>

namespace WikiMainPath {
	namespace GraphDrawing {
		std::vector<double> averaged_precessor_graph_drawing(const EventNetwork& event_network)
		{
			std::size_t n_wo_incoming = 0;
			for(std::size_t i = 0; i < boost::num_vertices(event_network); i++)
				if(boost::in_degree(i, event_network) == 0)
					n_wo_incoming++;

			double cur_pos = 0;
			std::vector<double> positions(boost::num_vertices(event_network), 0.0);
			for(std::size_t i = 0; i < boost::num_vertices(event_network); i++)
				if(boost::in_degree(i, event_network) == 0)
				{
					cur_pos += 1.0 / (n_wo_incoming+1);
					positions[i] = cur_pos;
				}

			// std::vector<EventNetwork::vertex_descriptor> topological_order;
			// boost::topological_sort(event_network, std::back_inserter(topological_order));

			// for(std::size_t v = 0; v < boost::num_vertices(event_network); v++)
			for(auto v : boost::make_iterator_range(boost::vertices(event_network))) // output from boost::topological_sort is in reverse top. order => reverse again
			// for(auto v : boost::adaptors::reverse(topological_order)) // output from boost::topological_sort is in reverse top. order => reverse again
				if(boost::in_degree(v, event_network) == 0)
					continue;
				else {
					// compute average of incoming neighbours
					double avg = 0.0;
					for (auto e : boost::make_iterator_range(boost::in_edges(v,event_network)))
						avg += positions[boost::source(e,event_network)];
					avg = avg / boost::in_degree(v,event_network);

					positions[v] = avg;
				}

			return positions;
		}

	}
}


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

/* Author: Luigi Palmieri */
#define BOOST_TEST_MODULE "HaltonDeterministicSampling"
#include <boost/test/unit_test.hpp>
#include <boost/filesystem.hpp>

#include <ompl/config.h>
#include <ompl/base/samplers/deterministic/HaltonSequence.h>
#include "../resources/haltonXD.h"

#include <iostream>

namespace ob = ompl::base;

BOOST_AUTO_TEST_CASE(Halton_1D)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);
    // Reading sequence from file
    HaltonXD hd1 = HaltonXD(1);
    hd1.loadSequence((path / "halton/halton_1d.txt").string().c_str());
    std::vector<std::vector<double>> seq = hd1.getSequence();
    // Defining Halton sequence from ompl::base
    ob::HaltonSequence hs1d(1);
    // checking if we have read all rows
    BOOST_CHECK_EQUAL(seq.size(), 5);
    // checking the samples of the rows
    for (unsigned int i = 0; i < 5; ++i)
    {
        std::vector<double> sample = hs1d.sample();
        std::vector<double> read_sample = seq[i];
        BOOST_CHECK_EQUAL(sample.size(), read_sample.size());

        for (unsigned int j = 0; j < sample.size(); j++)
        {
            BOOST_CHECK_CLOSE(sample[j], read_sample[j], 0.001);
        }
    }
}

BOOST_AUTO_TEST_CASE(Halton_2D)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);
    // Reading sequence from file
    HaltonXD hd2 = HaltonXD(2);
    hd2.loadSequence((path / "halton/halton_2d.txt").string().c_str());
    std::vector<std::vector<double>> seq = hd2.getSequence();
    // Defining Halton sequence from ompl::base
    ob::HaltonSequence hs2d(2);
    // checking if we have read all rows
    BOOST_CHECK_EQUAL(seq.size(), 5);
    // checking the samples of the rows
    for (unsigned int i = 0; i < 5; ++i)
    {
        std::vector<double> sample = hs2d.sample();
        std::vector<double> read_sample = seq[i];
        BOOST_CHECK_EQUAL(sample.size(), read_sample.size());

        for (unsigned int j = 0; j < sample.size(); j++)
        {
            BOOST_CHECK_CLOSE(sample[j], read_sample[j], 0.001);
        }
    }
}

BOOST_AUTO_TEST_CASE(Halton_5D)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);
    // Reading sequence from file
    HaltonXD hd5 = HaltonXD(5);
    hd5.loadSequence((path / "halton/halton_5d.txt").string().c_str());
    std::vector<std::vector<double>> seq = hd5.getSequence();
    // Defining Halton sequence from ompl::base
    ob::HaltonSequence hs5d(5);
    // checking if we have read all rows
    BOOST_CHECK_EQUAL(seq.size(), 5);
    // checking the samples of the rows
    for (unsigned int i = 0; i < 5; ++i)
    {
        std::vector<double> sample = hs5d.sample();
        std::vector<double> read_sample = seq[i];
        BOOST_CHECK_EQUAL(sample.size(), read_sample.size());

        for (unsigned int j = 0; j < sample.size(); j++)
        {
            BOOST_CHECK_CLOSE(sample[j], read_sample[j], 0.001);
        }
    }
}

BOOST_AUTO_TEST_CASE(Halton_10D)
{
    boost::filesystem::path path(TEST_RESOURCES_DIR);
    // Reading sequence from file
    HaltonXD hd10 = HaltonXD(10);
    hd10.loadSequence((path / "halton/halton_10d.txt").string().c_str());
    std::vector<std::vector<double>> seq = hd10.getSequence();
    // Defining Halton sequence from ompl::base
    ob::HaltonSequence hs10d(10);
    // checking if we have read all rows
    BOOST_CHECK_EQUAL(seq.size(), 5);
    // checking the samples of the rows
    for (unsigned int i = 0; i < 5; ++i)
    {
        std::vector<double> sample = hs10d.sample();
        std::vector<double> read_sample = seq[i];
        BOOST_CHECK_EQUAL(sample.size(), read_sample.size());

        for (unsigned int j = 0; j < sample.size(); j++)
        {
            BOOST_CHECK_CLOSE(sample[j], read_sample[j], 0.001);
        }
    }
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_CONTAINERS_MULTIARRAY_BLOCK_PARTITION_HPP
#define STAPL_CONTAINERS_MULTIARRAY_BLOCK_PARTITION_HPP

#include <boost/mpl/int.hpp>
#include <stapl/containers/partitions/ndim_partition.hpp>
#include <stapl/containers/partitions/balanced.hpp>
#include <stapl/domains/indexed.hpp>
#include <stapl/utility/tuple.hpp>
#include <cmath>
#include <algorithm>
#include <array>
#include <numeric>
#include <iterator>

#include "partitions_generator.hpp"

#include <stapl/containers/type_traits/is_invertible_partition.hpp>

namespace stapl {

namespace multiarray_impl {

////////////////////////////////////////////////////////////////////////
/// @brief Determines if a number is not prime.
////////////////////////////////////////////////////////////////////////
struct not_prime
{
  bool operator()(size_t x) const
  {
    for (size_t i = floor(sqrt(x)); i >= 2; --i)
    {
      if (x % i == 0 && i != x)
        return true;
    }

    return false;
  }
};

////////////////////////////////////////////////////////////////////////
/// @brief Function object that initializes a tuple of n elements that is
/// used to create an n-dimensional multiarray partition.
////////////////////////////////////////////////////////////////////////
template<size_t N, typename = make_index_sequence<N>>
struct make_multiarray_size;

template <size_t N, std::size_t... Indices>
struct make_multiarray_size<N, index_sequence<Indices...>>
{
  using result_type = typename homogeneous_tuple_type<N, size_t>::type;

  //////////////////////////////////////////////////////////////////////
  /// @brief Partitions the elements of the multiarray appropriately in
  /// each dimension.
  /// @param p The number of processors.
  //////////////////////////////////////////////////////////////////////
  result_type operator()(size_t p) const
  {
    std::array<size_t, N> v;

    v[0] = p;

    std::fill(v.begin() + 1, v.end(), 1);

    auto equal_one = [](size_t const& val) { return val == 1; };
    auto prime     = [](size_t const& val) { return !not_prime()(val); };

    while (std::count_if(v.begin(), v.end(), equal_one) > 0
        && std::count_if(v.begin(), v.end(), not_prime()) > 0)
    {
      auto filtered = std::partition(v.begin(), v.end(), prime);
      auto val1     = std::max_element(filtered, v.end());
      auto val2     = std::find_if(v.begin(), v.end(), equal_one);

      for (size_t i = floor(sqrt(*val1)); i >= 2; i--)
      {
        if (*val1 % i == 0) {
          *val2 = *val1/i;
          *val1 = i;
          break;
        }
      }
    }

    return result_type(get<N-Indices-1>(v)...);
  }
}; // struct make_multiarray_size

//////////////////////////////////////////////////////////////////////
/// @brief n-dimensional partition consisting of balanced_partitions of
/// indexed_domain. This class will partition a multidimensional domain
/// into multidimensional blocks with a certain number of blocks in each
/// dimension. This is the default partition for the @ref multiarray
/// container.
///
/// @tparam Traversal Multidimensional traversal type
/// @see multiarray
//////////////////////////////////////////////////////////////////////
template <typename Traversal>
class block_partition
  : public nd_partition<
      typename homogeneous_tuple_type<
        tuple_size<Traversal>::value,
        balanced_partition<indexed_domain<size_t>>>::type,
      Traversal>
{
private:
  /// The number of dimensions for this partition
  using dimension_type = boost::mpl::int_<tuple_size<Traversal>::value>;
  using base_type      = nd_partition<
                           typename homogeneous_tuple_type<
                             tuple_size<Traversal>::value,
                             balanced_partition<indexed_domain<size_t>>>::type,
                           Traversal>;

public:
  using partitions_type = typename base_type::partitions_type;

  //////////////////////////////////////////////////////////////////////
  /// @brief Create a multidimensional partition based on a given n-dimensional
  /// domain that is balanced based on the number of locations in each
  /// dimension.
  /// @param dom The original domain to partition.
  //////////////////////////////////////////////////////////////////////
  template<typename Dom>
  explicit
  block_partition(const Dom& dom)
    : base_type(partitions_impl::partitions_generator<
                  tuple_size<Traversal>::value, Dom, partitions_type
                >(dom)(partitions_type()))
  { }

  //////////////////////////////////////////////////////////////////////
  /// @brief Create a multidimensional partition based on a given n-dimensional
  /// domain and a tuple of the number of partitions in each dimension.
  /// @param dom The original domain to partition.
  /// @param nparts Tuple of the number of partitions in each dimension
  //////////////////////////////////////////////////////////////////////
  template<typename Dom, typename NParts>
  block_partition(Dom const& dom, NParts const& nparts)
    : base_type(partitions_impl::partitions_generator<
                  tuple_size<Traversal>::value, Dom, partitions_type
                >(dom)(partitions_type(), nparts))
  { }
}; // class block_partition

} // namespace multiarray_impl

template<typename Traversal>
struct is_invertible_partition<multiarray_impl::block_partition<Traversal>>
  : public std::integral_constant<bool, true>
{ };

} // namespace stapl

#endif // STAPL_CONTAINERS_MULTIARRAY_BLOCK_PARTITION_HPP