#ifndef STATIC_RTREE_HPP
#define STATIC_RTREE_HPP

#include "util/bearing.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/deallocating_vector.hpp"
#include "util/exception.hpp"
#include "util/hilbert_value.hpp"
#include "util/integer_range.hpp"
#include "util/rectangle.hpp"
#include "util/shared_memory_vector_wrapper.hpp"
#include "util/typedefs.hpp"
#include "util/web_mercator.hpp"

#include "osrm/coordinate.hpp"

#include <boost/assert.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/format.hpp>
#include <boost/iostreams/device/mapped_file.hpp>

#include <tbb/parallel_for.h>
#include <tbb/parallel_sort.h>

#include <algorithm>
#include <array>
#include <limits>
#include <memory>
#include <queue>
#include <string>
#include <vector>

// An extended alignment is implementation-defined, so use compiler attributes
// until alignas(LEAF_PAGE_SIZE) is compiler-independent.
#if defined(_MSC_VER)
#define ALIGNED(x) __declspec(align(x))
#elif defined(__GNUC__)
#define ALIGNED(x) __attribute__((aligned(x)))
#else
#define ALIGNED(x)
#endif

namespace osrm
{
namespace util
{

// Static RTree for serving nearest neighbour queries
// All coordinates are pojected first to Web Mercator before the bounding boxes
// are computed, this means the internal distance metric doesn not represent meters!
template <class EdgeDataT,
          class CoordinateListT = std::vector<Coordinate>,
          bool UseSharedMemory = false,
          std::uint32_t BRANCHING_FACTOR = 128,
          std::uint32_t LEAF_PAGE_SIZE = 4096>
class StaticRTree
{
  public:
    using Rectangle = RectangleInt2D;
    using EdgeData = EdgeDataT;
    using CoordinateList = CoordinateListT;

    static_assert(LEAF_PAGE_SIZE >= sizeof(uint32_t) + sizeof(EdgeDataT), "page size is too small");
    static_assert(((LEAF_PAGE_SIZE - 1) & LEAF_PAGE_SIZE) == 0, "page size is not a power of 2");
    static constexpr std::uint32_t LEAF_NODE_SIZE =
        (LEAF_PAGE_SIZE - sizeof(uint32_t) - sizeof(Rectangle)) / sizeof(EdgeDataT);

    struct CandidateSegment
    {
        Coordinate fixed_projected_coordinate;
        EdgeDataT data;
    };

    struct TreeIndex
    {
        TreeIndex() : index(0), is_leaf(false) {}
        TreeIndex(std::size_t index, bool is_leaf) : index(index), is_leaf(is_leaf) {}
        std::uint32_t index : 31;
        std::uint32_t is_leaf : 1;
    };

    struct TreeNode
    {
        TreeNode() : child_count(0) {}
        std::uint32_t child_count;
        Rectangle minimum_bounding_rectangle;
        TreeIndex children[BRANCHING_FACTOR];
    };

    struct ALIGNED(LEAF_PAGE_SIZE) LeafNode
    {
        LeafNode() : object_count(0), objects() {}
        std::uint32_t object_count;
        Rectangle minimum_bounding_rectangle;
        std::array<EdgeDataT, LEAF_NODE_SIZE> objects;
    };
    static_assert(sizeof(LeafNode) == LEAF_PAGE_SIZE, "LeafNode size does not fit the page size");

  private:
    struct WrappedInputElement
    {
        explicit WrappedInputElement(const uint64_t _hilbert_value,
                                     const std::uint32_t _array_index)
            : m_hilbert_value(_hilbert_value), m_array_index(_array_index)
        {
        }

        WrappedInputElement() : m_hilbert_value(0), m_array_index(UINT_MAX) {}

        uint64_t m_hilbert_value;
        std::uint32_t m_array_index;

        inline bool operator<(const WrappedInputElement &other) const
        {
            return m_hilbert_value < other.m_hilbert_value;
        }
    };

    struct QueryCandidate
    {
        QueryCandidate(std::uint64_t squared_min_dist, TreeIndex tree_index)
            : squared_min_dist(squared_min_dist), tree_index(tree_index),
              segment_index(std::numeric_limits<std::uint32_t>::max())
        {
        }

        QueryCandidate(std::uint64_t squared_min_dist,
                       TreeIndex tree_index,
                       std::uint32_t segment_index,
                       const Coordinate &coordinate)
            : squared_min_dist(squared_min_dist), tree_index(tree_index),
              segment_index(segment_index), fixed_projected_coordinate(coordinate)
        {
        }

        inline bool is_segment() const
        {
            return segment_index != std::numeric_limits<std::uint32_t>::max();
        }

        inline bool operator<(const QueryCandidate &other) const
        {
            // Attn: this is reversed order. std::pq is a max pq!
            return other.squared_min_dist < squared_min_dist;
        }

        std::uint64_t squared_min_dist;
        TreeIndex tree_index;
        std::uint32_t segment_index;
        Coordinate fixed_projected_coordinate;
    };

    typename ShM<TreeNode, UseSharedMemory>::vector m_search_tree;
    const CoordinateListT &m_coordinate_list;

    boost::iostreams::mapped_file_source m_leaves_region;
    // read-only view of leaves
    typename ShM<const LeafNode, true>::vector m_leaves;

  public:
    StaticRTree(const StaticRTree &) = delete;
    StaticRTree &operator=(const StaticRTree &) = delete;

    template <typename CoordinateT>
    // Construct a packed Hilbert-R-Tree with Kamel-Faloutsos algorithm [1]
    explicit StaticRTree(const std::vector<EdgeDataT> &input_data_vector,
                         const std::string &tree_node_filename,
                         const std::string &leaf_node_filename,
                         const std::vector<CoordinateT> &coordinate_list)
        : m_coordinate_list(coordinate_list)
    {
        const uint64_t element_count = input_data_vector.size();
        std::vector<WrappedInputElement> input_wrapper_vector(element_count);

        // generate auxiliary vector of hilbert-values
        tbb::parallel_for(
            tbb::blocked_range<uint64_t>(0, element_count),
            [&input_data_vector, &input_wrapper_vector, this](
                const tbb::blocked_range<uint64_t> &range) {
                for (uint64_t element_counter = range.begin(), end = range.end();
                     element_counter != end;
                     ++element_counter)
                {
                    WrappedInputElement &current_wrapper = input_wrapper_vector[element_counter];
                    current_wrapper.m_array_index = element_counter;

                    EdgeDataT const &current_element = input_data_vector[element_counter];

                    // Get Hilbert-Value for centroid in mercartor projection
                    BOOST_ASSERT(current_element.u < m_coordinate_list.size());
                    BOOST_ASSERT(current_element.v < m_coordinate_list.size());

                    Coordinate current_centroid = coordinate_calculation::centroid(
                        m_coordinate_list[current_element.u], m_coordinate_list[current_element.v]);
                    current_centroid.lat = FixedLatitude{static_cast<std::int32_t>(
                        COORDINATE_PRECISION *
                        web_mercator::latToY(toFloating(current_centroid.lat)))};

                    current_wrapper.m_hilbert_value = hilbertCode(current_centroid);
                }
            });

        // open leaf file
        boost::filesystem::ofstream leaf_node_file(leaf_node_filename, std::ios::binary);

        // sort the hilbert-value representatives
        tbb::parallel_sort(input_wrapper_vector.begin(), input_wrapper_vector.end());
        std::vector<TreeNode> tree_nodes_in_level;

        // pack M elements into leaf node, write to leaf file and add child index to the parent node
        uint64_t wrapped_element_index = 0;
        for (std::uint32_t node_index = 0; wrapped_element_index < element_count; ++node_index)
        {
            TreeNode current_node;
            for (std::uint32_t leaf_index = 0;
                 leaf_index < BRANCHING_FACTOR && wrapped_element_index < element_count;
                 ++leaf_index)
            {
                LeafNode current_leaf;
                Rectangle &rectangle = current_leaf.minimum_bounding_rectangle;
                for (std::uint32_t object_index = 0;
                     object_index < LEAF_NODE_SIZE && wrapped_element_index < element_count;
                     ++object_index, ++wrapped_element_index)
                {
                    const std::uint32_t input_object_index =
                        input_wrapper_vector[wrapped_element_index].m_array_index;
                    const EdgeDataT &object = input_data_vector[input_object_index];

                    current_leaf.object_count += 1;
                    current_leaf.objects[object_index] = object;

                    Coordinate projected_u{
                        web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.u]})};
                    Coordinate projected_v{
                        web_mercator::fromWGS84(Coordinate{m_coordinate_list[object.v]})};

                    BOOST_ASSERT(std::abs(toFloating(projected_u.lon).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_u.lat).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_v.lon).operator double()) <= 180.);
                    BOOST_ASSERT(std::abs(toFloating(projected_v.lat).operator double()) <= 180.);

                    rectangle.min_lon =
                        std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon));
                    rectangle.max_lon =
                        std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon));

                    rectangle.min_lat =
                        std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat));
                    rectangle.max_lat =
                        std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat));

                    BOOST_ASSERT(rectangle.IsValid());
                }

                // append the leaf node to the current tree node
                current_node.child_count += 1;
                current_node.children[leaf_index] =
                    TreeIndex{node_index * BRANCHING_FACTOR + leaf_index, true};
                current_node.minimum_bounding_rectangle.MergeBoundingBoxes(
                    current_leaf.minimum_bounding_rectangle);

                // write leaf_node to leaf node file
                leaf_node_file.write((char *)&current_leaf, sizeof(current_leaf));
            }

            tree_nodes_in_level.emplace_back(current_node);
        }
        leaf_node_file.flush();
        leaf_node_file.close();

        std::uint32_t processing_level = 0;
        while (1 < tree_nodes_in_level.size())
        {
            std::vector<TreeNode> tree_nodes_in_next_level;
            std::uint32_t processed_tree_nodes_in_level = 0;
            while (processed_tree_nodes_in_level < tree_nodes_in_level.size())
            {
                TreeNode parent_node;
                // pack BRANCHING_FACTOR elements into tree_nodes each
                for (std::uint32_t current_child_node_index = 0;
                     current_child_node_index < BRANCHING_FACTOR;
                     ++current_child_node_index)
                {
                    if (processed_tree_nodes_in_level < tree_nodes_in_level.size())
                    {
                        TreeNode &current_child_node =
                            tree_nodes_in_level[processed_tree_nodes_in_level];
                        // add tree node to parent entry
                        parent_node.children[current_child_node_index] =
                            TreeIndex{m_search_tree.size(), false};
                        m_search_tree.emplace_back(current_child_node);
                        // merge MBRs
                        parent_node.minimum_bounding_rectangle.MergeBoundingBoxes(
                            current_child_node.minimum_bounding_rectangle);
                        // increase counters
                        ++parent_node.child_count;
                        ++processed_tree_nodes_in_level;
                    }
                }
                tree_nodes_in_next_level.emplace_back(parent_node);
            }
            tree_nodes_in_level.swap(tree_nodes_in_next_level);
            ++processing_level;
        }
        BOOST_ASSERT_MSG(tree_nodes_in_level.size() == 1, "tree broken, more than one root node");
        // last remaining entry is the root node, store it
        m_search_tree.emplace_back(tree_nodes_in_level[0]);

        // reverse and renumber tree to have root at index 0
        std::reverse(m_search_tree.begin(), m_search_tree.end());

        std::uint32_t search_tree_size = m_search_tree.size();
        tbb::parallel_for(
            tbb::blocked_range<std::uint32_t>(0, search_tree_size),
            [this, &search_tree_size](const tbb::blocked_range<std::uint32_t> &range) {
                for (std::uint32_t i = range.begin(), end = range.end(); i != end; ++i)
                {
                    TreeNode &current_tree_node = this->m_search_tree[i];
                    for (std::uint32_t j = 0; j < current_tree_node.child_count; ++j)
                    {
                        if (!current_tree_node.children[j].is_leaf)
                        {
                            const std::uint32_t old_id = current_tree_node.children[j].index;
                            const std::uint32_t new_id = search_tree_size - old_id - 1;
                            current_tree_node.children[j].index = new_id;
                        }
                    }
                }
            });

        // open tree file
        boost::filesystem::ofstream tree_node_file(tree_node_filename, std::ios::binary);

        std::uint32_t size_of_tree = m_search_tree.size();
        BOOST_ASSERT_MSG(0 < size_of_tree, "tree empty");
        tree_node_file.write((char *)&size_of_tree, sizeof(std::uint32_t));
        tree_node_file.write((char *)&m_search_tree[0], sizeof(TreeNode) * size_of_tree);

        MapLeafNodesFile(leaf_node_filename);
    }

    explicit StaticRTree(const boost::filesystem::path &node_file,
                         const boost::filesystem::path &leaf_file,
                         const CoordinateListT &coordinate_list)
        : m_coordinate_list(coordinate_list)
    {
        // open tree node file and load into RAM.
        if (!boost::filesystem::exists(node_file))
        {
            throw exception("ram index file does not exist");
        }
        if (boost::filesystem::file_size(node_file) == 0)
        {
            throw exception("ram index file is empty");
        }
        boost::filesystem::ifstream tree_node_file(node_file, std::ios::binary);

        std::uint32_t tree_size = 0;
        tree_node_file.read((char *)&tree_size, sizeof(std::uint32_t));

        m_search_tree.resize(tree_size);
        if (tree_size > 0)
        {
            tree_node_file.read((char *)&m_search_tree[0], sizeof(TreeNode) * tree_size);
        }

        MapLeafNodesFile(leaf_file);
    }

    explicit StaticRTree(TreeNode *tree_node_ptr,
                         const uint64_t number_of_nodes,
                         const boost::filesystem::path &leaf_file,
                         const CoordinateListT &coordinate_list)
        : m_search_tree(tree_node_ptr, number_of_nodes), m_coordinate_list(coordinate_list)
    {
        MapLeafNodesFile(leaf_file);
    }

    void MapLeafNodesFile(const boost::filesystem::path &leaf_file)
    {
        // open leaf node file and return a pointer to the mapped leaves data
        try
        {
            m_leaves_region.open(leaf_file);
            std::size_t num_leaves = m_leaves_region.size() / sizeof(LeafNode);
            m_leaves.reset(reinterpret_cast<const LeafNode *>(m_leaves_region.data()), num_leaves);
        }
        catch (const std::exception &exc)
        {
            throw exception(boost::str(boost::format("Leaf file %1% mapping failed: %2%") %
                                       leaf_file % exc.what()));
        }
    }

    /* Returns all features inside the bounding box.
       Rectangle needs to be projected!*/
    std::vector<EdgeDataT> SearchInBox(const Rectangle &search_rectangle) const
    {
        const Rectangle projected_rectangle{
            search_rectangle.min_lon,
            search_rectangle.max_lon,
            toFixed(FloatLatitude{
                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.min_lat)))}),
            toFixed(FloatLatitude{
                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.max_lat)))})};
        std::vector<EdgeDataT> results;

        std::queue<TreeIndex> traversal_queue;
        traversal_queue.push(TreeIndex{});

        while (!traversal_queue.empty())
        {
            auto const current_tree_index = traversal_queue.front();
            traversal_queue.pop();

            if (current_tree_index.is_leaf)
            {
                const LeafNode &current_leaf_node = m_leaves[current_tree_index.index];

                for (const auto i : irange(0u, current_leaf_node.object_count))
                {
                    const auto &current_edge = current_leaf_node.objects[i];

                    // we don't need to project the coordinates here,
                    // because we use the unprojected rectangle to test against
                    const Rectangle bbox{std::min(m_coordinate_list[current_edge.u].lon,
                                                  m_coordinate_list[current_edge.v].lon),
                                         std::max(m_coordinate_list[current_edge.u].lon,
                                                  m_coordinate_list[current_edge.v].lon),
                                         std::min(m_coordinate_list[current_edge.u].lat,
                                                  m_coordinate_list[current_edge.v].lat),
                                         std::max(m_coordinate_list[current_edge.u].lat,
                                                  m_coordinate_list[current_edge.v].lat)};

                    // use the _unprojected_ input rectangle here
                    if (bbox.Intersects(search_rectangle))
                    {
                        results.push_back(current_edge);
                    }
                }
            }
            else
            {
                const TreeNode &current_tree_node = m_search_tree[current_tree_index.index];

                // If it's a tree node, look at all children and add them
                // to the search queue if their bounding boxes intersect
                for (std::uint32_t i = 0; i < current_tree_node.child_count; ++i)
                {
                    const TreeIndex child_id = current_tree_node.children[i];
                    const auto &child_rectangle =
                        child_id.is_leaf ? m_leaves[child_id.index].minimum_bounding_rectangle
                                         : m_search_tree[child_id.index].minimum_bounding_rectangle;

                    if (child_rectangle.Intersects(projected_rectangle))
                    {
                        traversal_queue.push(child_id);
                    }
                }
            }
        }
        return results;
    }

    // Override filter and terminator for the desired behaviour.
    std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate,
                                   const std::size_t max_results) const
    {
        return Nearest(input_coordinate,
                       [](const CandidateSegment &) { return std::make_pair(true, true); },
                       [max_results](const std::size_t num_results, const CandidateSegment &) {
                           return num_results >= max_results;
                       });
    }

    // Override filter and terminator for the desired behaviour.
    template <typename FilterT, typename TerminationT>
    std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate,
                                   const FilterT filter,
                                   const TerminationT terminate) const
    {
        std::vector<EdgeDataT> results;
        auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
        Coordinate fixed_projected_coordinate{projected_coordinate};

        // initialize queue with root element
        std::priority_queue<QueryCandidate> traversal_queue;
        traversal_queue.push(QueryCandidate{0, TreeIndex{}});

        while (!traversal_queue.empty())
        {
            QueryCandidate current_query_node = traversal_queue.top();
            traversal_queue.pop();

            const TreeIndex &current_tree_index = current_query_node.tree_index;
            if (!current_query_node.is_segment())
            { // current object is a tree node
                if (current_tree_index.is_leaf)
                {
                    ExploreLeafNode(current_tree_index,
                                    fixed_projected_coordinate,
                                    projected_coordinate,
                                    traversal_queue);
                }
                else
                {
                    ExploreTreeNode(
                        current_tree_index, fixed_projected_coordinate, traversal_queue);
                }
            }
            else
            { // current candidate is an actual road segment
                auto edge_data =
                    m_leaves[current_tree_index.index].objects[current_query_node.segment_index];
                const auto &current_candidate =
                    CandidateSegment{current_query_node.fixed_projected_coordinate, edge_data};

                // to allow returns of no-results if too restrictive filtering, this needs to be
                // done here even though performance would indicate that we want to stop after
                // adding the first candidate
                if (terminate(results.size(), current_candidate))
                {
                    break;
                }

                auto use_segment = filter(current_candidate);
                if (!use_segment.first && !use_segment.second)
                {
                    continue;
                }
                edge_data.forward_segment_id.enabled &= use_segment.first;
                edge_data.reverse_segment_id.enabled &= use_segment.second;

                // store phantom node in result vector
                results.push_back(std::move(edge_data));
            }
        }

        return results;
    }

  private:
    template <typename QueueT>
    void ExploreLeafNode(const TreeIndex &leaf_id,
                         const Coordinate &projected_input_coordinate_fixed,
                         const FloatCoordinate &projected_input_coordinate,
                         QueueT &traversal_queue) const
    {
        const LeafNode &current_leaf_node = m_leaves[leaf_id.index];

        // current object represents a block on disk
        for (const auto i : irange(0u, current_leaf_node.object_count))
        {
            const auto &current_edge = current_leaf_node.objects[i];
            const auto projected_u = web_mercator::fromWGS84(m_coordinate_list[current_edge.u]);
            const auto projected_v = web_mercator::fromWGS84(m_coordinate_list[current_edge.v]);

            FloatCoordinate projected_nearest;
            std::tie(std::ignore, projected_nearest) =
                coordinate_calculation::projectPointOnSegment(
                    projected_u, projected_v, projected_input_coordinate);

            const auto squared_distance = coordinate_calculation::squaredEuclideanDistance(
                projected_input_coordinate_fixed, projected_nearest);
            // distance must be non-negative
            BOOST_ASSERT(0. <= squared_distance);
            traversal_queue.push(
                QueryCandidate{squared_distance, leaf_id, i, Coordinate{projected_nearest}});
        }
    }

    template <class QueueT>
    void ExploreTreeNode(const TreeIndex &parent_id,
                         const Coordinate &fixed_projected_input_coordinate,
                         QueueT &traversal_queue) const
    {
        const TreeNode &parent = m_search_tree[parent_id.index];
        for (std::uint32_t i = 0; i < parent.child_count; ++i)
        {
            const TreeIndex child_id = parent.children[i];
            const auto &child_rectangle =
                child_id.is_leaf ? m_leaves[child_id.index].minimum_bounding_rectangle
                                 : m_search_tree[child_id.index].minimum_bounding_rectangle;
            const auto squared_lower_bound_to_element =
                child_rectangle.GetMinSquaredDist(fixed_projected_input_coordinate);
            traversal_queue.push(QueryCandidate{squared_lower_bound_to_element, child_id});
        }
    }
};

//[1] "On Packing R-Trees"; I. Kamel, C. Faloutsos; 1993; DOI: 10.1145/170088.170403
//[2] "Nearest Neighbor Queries", N. Roussopulos et al; 1995; DOI: 10.1145/223784.223794
//[3] "Distance Browsing in Spatial Databases"; G. Hjaltason, H. Samet; 1999; ACM Trans. DB Sys
// Vol.24 No.2, pp.265-318
}
}

#endif // STATIC_RTREE_HPP


#pragma once
#include <memory>
#include <Eigen/Dense>

namespace orbit
{

	using namespace Eigen;

    // @brief: the type of light.
	enum class LightType
	{
		// @brief: a directional light is only defined by the direction and the color of the light
		//	the sun is a good example of a directional light. We do not care for the position of the sun,
		//	only the direction of its light rays matters
		// This is the fastest light to compute
		DirectionalLight,
		// @brief: a point light is a perfect light bulb, where you have a light source at a specific position
		//	and rays are going of in all directions (in contrast to the directional light)
		// Slower to compute than the directional light
		PointLight,
		// @brief: a spot light can be thought of as a flash light, where you have not a sphere of light
		//	but only a cone of light being emitted from a certain point with a certain angle
		// Spot lights are the most expensive lights to compute
		SpotLight
	};

	// @brief: holds information about a light in the scene
	//	therefore this struct holds information that all of the light types need
	struct Light
	{
		bool _enabled = false;
		// @member: the color of the light. All lights share this property
		Vector4f _color = Vector4f::Ones();
		// @member: the position of the light. Only point- and spotlights
		Vector4f _position = Vector4f::Zero();
		// @member: the direction of the light rays. Only directional- and spotlights
		Vector4f _direction = Vector4f::Zero();
		// @member: the angle of a spotlight cone. Only spotlights
		float _spotAngle = 0.f;
		// @member: the distance at which to begin dimming the light. Only point- and spotlights
		float _falloffBegin = 0.f;
		// @member: the maximum distance of a light source. Only point- and spotlights
		float _falloffEnd = 1.f;
		// @member: the type of the light
		LightType _type = LightType::PointLight;

		// @method: creates a new directional light
		// @param color: the color of the light
		// @param direction: the direction of the light
		// @return: a directional light that can be inserted into a scene
		static std::shared_ptr<Light> CreateDirectionalLight(
			const Vector4f& color,
			const Vector4f& direction
		);

		// @method: creates a new point light
		// @param color: the color of the light
		// @param position: the position of the light
		// @param falloffBegin: the distance at which to begin dimming the light
		// @param falloffEnd: the maximum distance of the light source's rays
		// @return: a point light that can be inserted into a scene
		static std::shared_ptr<Light> CreatePointLight(
			const Vector4f& color,
			const Vector4f& position,
			float falloffBegin,
			float falloffEnd
		);

		// @method: creates a new spot light
		// @param color: the color of the light
		// @param position: the position of the light
		// @param direction: the direction of the light
		// @param spotAngle: the angle of the light cone
		// @param falloffBegin: the distance at which to begin dimming the light
		// @param falloffEnd: the maximum distance of the light source's rays
		// @return: a point light that can be inserted into a scene
		static std::shared_ptr<Light> CreateSpotLight(
			const Vector4f& color,
			const Vector4f& position,
			const Vector4f& direction,
			float spotAngle,
			float falloffBegin,
			float falloffEnd
		);
	};

	using LightPtr = std::shared_ptr<Light>;

}


#include <ros/ros.h>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/conversions.h>
#include <pcl/common/common_headers.h>
#include <pcl/common/angles.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <fstream>
#include <boost/make_shared.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>

typedef pcl::PointXYZRGB Point;
bool flag;

class Calibrator
{
  ros::NodeHandle nh;
  ros::Subscriber cloud_sub;
public:

  Calibrator(ros::NodeHandle nh_): nh(nh_)
  {
    cloud_sub = nh.subscribe("/kinect2/qhd/points", 1,
                             &Calibrator::cloud_cb, this);
  }

  void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
  {
    pcl::PointCloud<Point>::Ptr cloud_input(new pcl::PointCloud<Point>);
    pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>);
    pcl::PointCloud<Point>::Ptr cloud_downsampled(new pcl::PointCloud<Point>);

    pcl::fromROSMsg(*input, *cloud_input);

    pcl::PassThrough<Point> pass_filter;
    pass_filter.setInputCloud(cloud_input);
    pass_filter.setFilterFieldName("z");
    pass_filter.setFilterLimits(0.0, 2.7);
    pass_filter.filter(*cloud_filtered);

    pcl::VoxelGrid<Point> voxel;
    voxel.setInputCloud(cloud_filtered);
    voxel.setLeafSize(0.01, 0.01, 0.01);
    voxel.setDownsampleAllData(true);
    voxel.filter(*cloud_downsampled);

    pcl::ModelCoefficients::Ptr coefficients_floor(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_floor(new pcl::PointIndices);
    pcl::SACSegmentation<Point> seg;
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setOptimizeCoefficients(true);
    seg.setMaxIterations(500);
    seg.setDistanceThreshold(0.02);
    seg.setInputCloud(cloud_downsampled);
    seg.segment(*inliers_floor, *coefficients_floor);
    Eigen::Vector3f XZ_norm(0.0, -1.0, 0.0);
    Eigen::Vector3f floor_norm(coefficients_floor->values[0], coefficients_floor->values[1],
        coefficients_floor->values[2]);
    float angle = acos(floor_norm.dot(XZ_norm));
    std::fstream f;
    f.open("cal_data.dat");
    // File has not been created
    if (!f.is_open())
    {
      f.open("cal_data.dat", std::ios::out|std::ios::in|std::ios::trunc);
      if (!f.is_open())
      {
        std::cerr << "Error while creating calibration file.\n";
        f.close();
        return;
      }
      f << angle;
      std::cerr << "File has been created.\n";
    }
    else // File already exists
    {
      std::cerr << "Changing angle value in calibration file.\n";
      f.clear();
      f.seekg(0);
      f << angle;
    }
    f.close();
    flag = false;
  }
};

int main(int argc, char **argv)
{
  flag = true;
  ros::init(argc, argv, "calibration_node");
  ros::NodeHandle nh;
  Calibrator cal(nh);
  while (flag) {
    ros::spinOnce();
  }
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>

#include <cctbx/miller/lookup_utils.h>

namespace cctbx { namespace miller {

namespace{

  struct lookup_tensor_wrapper
  {
    typedef lookup_utils::lookup_tensor<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("lookup_tensor", no_init)
        .def(init< scitbx::af::const_ref< cctbx::miller::index<> >  const&,
                   cctbx::sgtbx::space_group const&,
                   bool const&
                 >((arg("miller_indices"),
                    arg("space_group"),
                    arg("anomalous_flag") )))
        .def("n_duplicates", &w_t::n_duplicates)
        .def("find_hkl", (long(w_t::*)(cctbx::miller::index<> const&) const)
             &w_t::find_hkl)
        .def("find_hkl", (scitbx::af::shared< long >(w_t::*)
                          (scitbx::af::const_ref< cctbx::miller::index<> >
                           const&) const)
             &w_t::find_hkl)
        ;
    }

  };

  struct local_neighbourhood_wrapper
  {
    typedef lookup_utils::local_neighbourhood<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("local_neighbourhood", no_init)
        .def(init< scitbx::af::const_ref< cctbx::miller::index<> > const&,
                   cctbx::sgtbx::space_group const&,
             bool const&,
                   long const&
                 >
             ((
                arg("miller_indices"),
                arg("space_group"),
                arg("anomalous_flag"),
                arg("radius")
                )))
        .def("construct_neighbourhood",
             ( std::vector<unsigned>(w_t::*) ( cctbx::miller::index<> const&) )
             &w_t::construct_neighbourhood)
        .def("construct_neighbourhood",
             ( scitbx::af::shared< std::vector<unsigned> >(w_t::*)
               ( scitbx::af::shared<cctbx::miller::index<> > const&) )
             &w_t::construct_neighbourhood)

        .def("construct_neighbourhood",
             ( std::vector<unsigned>(w_t::*) ( unsigned const&) )
             &w_t::construct_neighbourhood)
        .def("construct_neighbourhood",
             ( scitbx::af::shared< std::vector<unsigned> >(w_t::*)
               ( scitbx::af::shared< long > const&) )
             &w_t::construct_neighbourhood)
        .def("construct_neighbourhood",
             ( scitbx::af::shared< std::vector<unsigned> >(w_t::*)
               ( ) )
             &w_t::construct_neighbourhood)

        ;

        }
  };

  struct local_area
  {
    typedef lookup_utils::local_area<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("local_area", no_init)
        .def(init< scitbx::af::const_ref< cctbx::miller::index<> > const&,
                   scitbx::af::const_ref< bool > const&,
                   cctbx::sgtbx::space_group const&,
                   bool const&,
                   std::size_t const&,
                   std::size_t const&,
                   std::size_t const&
                 >
             ((
                arg("miller_indices"),
                arg("property"),
                arg("space_group"),
                arg("anomalous_flag"),
                arg("radius"),
                arg("depth"),
                arg("target_ref")
                )))
        .def("area", &w_t::area)
        ;

        }
  };

}  // namespace <anonymous>

namespace boost_python {

  void wrap_lookup_tensor()
  {
    lookup_tensor_wrapper::wrap();
  }

  void wrap_local_neighbourhood()
  {
    local_neighbourhood_wrapper::wrap();
  }

  void wrap_local_area()
  {
    local_area::wrap();
  }

}}}


#include "onlineforest.h"

#include "forest.h"
#include <string.h>
#include <fstream>
#include <boost/foreach.hpp>

OnlineForest::OnlineForest(const HyperParameters &hp)
{
  m_hp = &hp;
}

void OnlineForest::initialize(const int numSamples)
{

    m_confidences.resize(numSamples, m_hp->numClasses);
    m_predictions.resize(numSamples);

    for (int n = 0; n < numSamples; n++)
    {
        for (int m = 0; m < m_hp->numClasses; m++)
        {
            m_confidences(n, m) = 0;
        }
    }
}

void OnlineForest::train(const matrix<float>& data, const std::vector<int>& labels, bool use_gpu)
{
    // Initialize
    initialize(data.size1());

    if (m_hp->useGPU || use_gpu)
    {
        cerr << "ERROR: No GPU Implementation available for online forests!" << endl;
        //std::vector<double> weights(labels.size(),1);
        //trainByGPU(data,labels, weights);
    }
    else
    {
        trainByCPU(data,labels);
    }
}

OnlineForest::~OnlineForest()
{
    m_trees.clear();
}

void OnlineForest::eval(const matrix<float>& data, const std::vector<int>& labels, bool use_gpu)
{
    if (m_hp->useGPU || use_gpu)
    {
        cerr << "ERROR: No GPU Implementation available for online forests!" << endl;
        //evalByGPU(data, labels);
    }
    else
    {
        evalByCPU(data, labels);
    }
}

void OnlineForest::load(const std::string &name)
{
    std::string loadName;
    loadName = (name == "default") ? m_hp->loadName : name;
    // now load that stuff
}

// CPU penne code only below this line

int poisson(double A)
{
    int k = 0;
    int maxK = 10;
    while(1) {
        double U_k = _rand();
        A *= U_k;
        if( k > maxK || A < exp(-1.0) ) {
            break;
        }
        k++;
    }
    return k;
}

void OnlineForest::trainByCPU(const matrix<float>& data, const std::vector<int>& labels)
{
    HyperParameters tmpHP = *m_hp;
    tmpHP.verbose = false;

    m_trees.clear();
    // Create trees
    for (int tree = 0; tree < m_hp->numTrees; tree++)
    {
        m_trees.push_back(OnlineTree(tmpHP,(int)data.size2()));
    }

    if (m_hp->verbose)
    {
        cout << "Training a random forest with " << m_hp->numTrees << " , grab a coffee ... " << endl;
        cout << "\tEpoch #: ";
    }

    // Train Forest
    std::vector<int> randperm;
    for (int round = 0; round < m_hp->numOnlineEpochs; round++)
    {
        if (m_hp->verbose)
        {
            cout << round + 1 << " " << flush;
        }
        //randperm = randPerm(data.size1());
        randperm = randPerm(m_hp->numLabeled);
        for ( int s = 0; s < m_hp->numLabeled; s++)
        {
            std::vector<float> sample(data.size2(),0);
            for ( int feature = 0; feature < (int)data.size2(); feature++)
            {
                sample[feature] = data(randperm[s],feature);
            }
            std::vector<OnlineTree>::iterator treeIt(m_trees.begin());
            std::vector<OnlineTree>::iterator treeEnd(m_trees.end());
            while ( treeIt != treeEnd )
            {
                double k = poisson(1.0);
                for(int curK = 0; curK < k; curK++) {
                    treeIt->train(sample,labels[randperm[s]]);
                }
                ++treeIt;
            }
        }
        //cout << "bla" << endl;
    }
    if (m_hp->verbose)
    {
        cout << " Done." << endl;
    }

    // Eval Forest
    double error = 0.0;
    for ( int s = 0; s < m_hp->numLabeled;s++)
    {
        std::vector<float> sample(data.size2(),0);
        for ( int feature = 0; feature < (int)data.size2(); feature++)
        {
            sample[feature] = data(s,feature);
        }

        std::vector<int> predictions(m_hp->numClasses,0.0);
        std::vector<OnlineTree>::iterator treeIt(m_trees.begin());
        std::vector<OnlineTree>::iterator treeEnd(m_trees.end());
        while ( treeIt != treeEnd )
        {
            int prediction = (int)treeIt->eval(sample,labels[s]);
            predictions[prediction]++;
            ++treeIt;
        }

        int maxPrediction = 0;
        int bestClass = 0;
        int c = 0;
        BOOST_FOREACH(int pred,predictions){
            if(pred > maxPrediction){
                maxPrediction = pred;
                bestClass = c;
            }
            c++;
        }
        if (bestClass != labels[s])
        {
            error++;
        }
    }

    if (m_hp->verbose)
    {
        cout << "\tTraining Error: " << error/(double)m_hp->numLabeled << endl;
    }

}

void OnlineForest::evalByCPU(const matrix<float>& data, const std::vector<int>& labels)
{
   // Eval Forest
    double error = 0.0;
    for ( int s = 0; s < (int) data.size1();s++)
    {
        std::vector<float> sample(data.size2(),0);
        for ( int feature = 0; feature < (int)data.size2(); feature++)
        {
            sample[feature] = data(s,feature);
        }

        std::vector<int> predictions(m_hp->numClasses,0.0);
        std::vector<OnlineTree>::iterator treeIt(m_trees.begin());
        std::vector<OnlineTree>::iterator treeEnd(m_trees.end());
        while ( treeIt != treeEnd )
        {
            int prediction = (int)treeIt->eval(sample,labels[s]);
            predictions[prediction]++;
            ++treeIt;
        }

        int maxPrediction = 0;
        int bestClass = 0;
        int c = 0;
        BOOST_FOREACH(int pred,predictions){
            if(pred > maxPrediction){
                maxPrediction = pred;
                bestClass = c;
            }
            c++;
        }
        if (bestClass != labels[s])
        {
            error++;
        }
    }
    if(m_hp->verbose) {
        cout << "\tTest Error: " << error/(double) data.size1() << endl;
    }
}

double OnlineForest::computeError(const std::vector<int>& labels)
{
    int bestClass, nSamp = 0;
    float bestConf;
    double error = 0;
    BOOST_FOREACH(int pre, m_predictions)
    {
        bestClass = 0;
        bestConf = 0;
        for (int nClass = 0; nClass < (int) m_hp->numClasses; nClass++)
        {
            if (m_confidences(nSamp, nClass) > bestConf)
            {
                bestClass = nClass;
                bestConf = m_confidences(nSamp, nClass);
            }
        }

        pre = bestClass;
        if (bestClass != labels[nSamp])
        {
            error++;
        }

        nSamp++;
    }
    error /= (double) m_predictions.size();

    return error;
}

double OnlineForest::computeError(const std::vector<int>& labels, const matrix<float>& confidences,
                                  const std::vector<int>& voteNum)
{
    double error = 0, bestConf;
    int sampleNum = 0, bestClass;
    for (int nSamp = 0; nSamp < (int) confidences.size1(); nSamp++)
    {
        bestClass = 0;
        bestConf = 0;
        if (voteNum[nSamp])
        {
            sampleNum++;
            for (int nClass = 0; nClass < (int) m_hp->numClasses; nClass++)
            {
                if (confidences(nSamp, nClass) > bestConf)
                {
                    bestClass = nClass;
                    bestConf = confidences(nSamp, nClass);
                }
            }

            if (bestClass != labels[nSamp])
            {
                error++;
            }
        }
    }
    error /= (double) sampleNum;

    return error;
}

void OnlineForest::writeError(const std::string& fileName, double error)
{
    std::ofstream myfile;
    myfile.open(fileName.c_str(), std::ios::app);
    myfile << error << endl;
    myfile.flush();
    myfile.close();
}


/*
* TS Elements
* Copyright 2015-2018 M. Newhouse
* Released under the MIT license.
*/

#include "dynamic_scene.hpp"
#include "drawable_entity.hpp"

#include "world/entity.hpp"

#include "resources/color_scheme.hpp"

#include "utility/debug_log.hpp"
#include "utility/rotation.hpp"

#include "graphics/gl_check.hpp"

#ifndef GLM_ENABLE_EXPERIMENTAL
#define GLM_ENABLE_EXPERIMENTAL
#endif

#include <glm/mat4x4.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/transform.hpp>

#include <boost/optional.hpp>

#include <cmath>
#include <algorithm>
#include <array>
#include <iostream>

namespace ts
{
  namespace scene
  {
    namespace detail
    {
      static constexpr std::int32_t max_color_schemes = 256;

      struct Texture
      {
        std::unique_ptr<graphics::Texture> texture;
        Vector2i texture_size;
      };

      struct EntityModel
      {
        world::EntityType type;
        const graphics::Texture* texture;
        Vector2i texture_size;
        std::uint32_t num_rotations;

        Vector2f scale;
        std::vector<std::pair<Vector2f, Vector2f>> texture_coords;
        sf::Transform colorizer_transform;
      };

      struct EntityInstance
      {
        std::uintptr_t type_tag;
        const world::Entity* entity;
        Vector2<double> stored_position;
        std::size_t model_id;

        struct ColorScheme
        {
          std::uint16_t scheme_id = 0;
          std::array<Colorf, 3> colors;
        };

        boost::optional<ColorScheme> color_scheme;
      };

      // This function can be used to calculate the frame id given a rotation and number of frames,
      // and also gives back the amount that the rotation deviates from the frame's corresponding rotation.
      static auto calculate_frame_id(Rotation<double> rotation, std::uint32_t frame_count,
                                     Rotation<double>& deviation)
      {
        // Degrees in range [-180, 180]
        rotation.normalize();

        auto deg = rotation.degrees();
        auto dbl_frame_count = static_cast<double>(frame_count);

        auto rotation_range = 360.0 / dbl_frame_count;
        auto frame_id = std::round(deg / rotation_range);
        auto frame_rotation = degrees(frame_id * rotation_range);

        deviation = rotation - frame_rotation;

        if (frame_id < 0.0) frame_id += dbl_frame_count;
        return static_cast<std::int32_t>(frame_id);
      }
    }

    struct DynamicScene::Impl
    {
      std::vector<detail::Texture> textures;
      std::vector<detail::EntityModel> entity_models;
      std::vector<detail::EntityInstance> entities;

      std::vector<graphics::Texture> color_scheme_textures;
    };

    DynamicScene::DynamicScene()
      : impl_(std::make_unique<Impl>())
    {
      impl_->color_scheme_textures.reserve(detail::max_color_schemes);
    }

    DynamicScene::~DynamicScene() = default;

    DynamicScene::DynamicScene(DynamicScene&& other) = default;
    DynamicScene& DynamicScene::operator=(DynamicScene&& rhs) = default;

    std::size_t DynamicScene::register_texture(std::unique_ptr<graphics::Texture> texture, Vector2i size)
    {
      glCheck(glBindTexture(GL_TEXTURE_2D, texture->get()));
      glCheck(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR));
      glCheck(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR));
      glCheck(glBindTexture(GL_TEXTURE_2D, 0));

      std::size_t texture_id = impl_->textures.size();
      impl_->textures.push_back({ std::move(texture), size });

      return texture_id;
    }

    std::size_t DynamicScene::add_model(world::EntityType entity_type, std::size_t texture_id, const TextureInfo& texture_info)
    {
      std::size_t model_id = impl_->entity_models.size();

      auto texture_size = vector2_cast<float>(impl_->textures[texture_id].texture_size);
      auto inverse_texture_size = 1.0f / texture_size;
      auto frame_size = vector2_cast<float>(texture_info.frame_size);
      auto row_width = texture_info.texture_rect.width / texture_info.frame_size.x;

      detail::EntityModel model;
      model.type = entity_type;
      model.texture = impl_->textures[texture_id].texture.get();
      model.texture_size = impl_->textures[texture_id].texture_size;
      model.num_rotations = texture_info.num_rotations;
      model.scale = frame_size / static_cast<float>(texture_info.scale);

      if (texture_info.frame_bounds.width > 0 && texture_info.frame_bounds.height > 0)
      {
        auto frame_bounds = rect_cast<float>(texture_info.frame_bounds);
        auto inv_frame_size = 1.0 / frame_size;

        frame_bounds.left *= inv_frame_size.x;
        frame_bounds.top *= inv_frame_size.y;
        frame_bounds.width *= inv_frame_size.x;
        frame_bounds.height *= inv_frame_size.y;

        frame_bounds.left -= 0.5f;
        frame_bounds.top -= 0.5f;

        auto center = make_vector2(frame_bounds.left + frame_bounds.width * 0.5f,
                                   frame_bounds.top + frame_bounds.height * 0.5f);

        model.colorizer_transform.
          translate(-center.x + 0.5, -center.y + 0.5).
          scale(1.0 / frame_bounds.width, 1.0 / frame_bounds.height);
      }

      model.texture_coords.resize(texture_info.num_rotations);
      for (std::uint32_t frame_id = 0; frame_id < texture_info.num_rotations; ++frame_id)
      {
        auto texture_position = make_vector2(frame_id % row_width, frame_id / row_width) * texture_info.frame_size;

        auto& coords = model.texture_coords[frame_id];
        coords.second = vector2_cast<float>(texture_info.frame_size) * inverse_texture_size;
        coords.first = vector2_cast<float>(texture_position) * inverse_texture_size + coords.second * 0.5f;
      }

      impl_->entity_models.push_back(std::move(model));
      return model_id;
    }

    std::uint32_t DynamicScene::register_color_scheme(graphics::Texture texture)
    {
      auto& textures = impl_->color_scheme_textures;
      if (textures.size() == textures.capacity()) return static_cast<std::uint32_t>(-1);

      glBindTexture(GL_TEXTURE_2D, texture.get());
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

      auto id = static_cast<std::uint32_t>(textures.size());
      textures.push_back(std::move(texture));
      return id;
    }

    void DynamicScene::add_entity(const world::Entity* entity, std::size_t model_id)
    {
      detail::EntityInstance instance;
      instance.entity = entity;
      instance.model_id = model_id;
      instance.stored_position = entity->position();

      impl_->entities.push_back(instance);
    }

    void DynamicScene::add_entity(const world::Entity* entity, std::size_t model_id, const resources::ColorScheme& color_scheme)
    {
      detail::EntityInstance instance;
      instance.entity = entity;
      instance.model_id = model_id;
      instance.stored_position = entity->position();
      instance.color_scheme.emplace();
      instance.color_scheme->scheme_id = color_scheme.scheme_id;
      std::transform(color_scheme.colors.begin(), color_scheme.colors.end(), instance.color_scheme->colors.begin(),
                     [](auto color) { return to_colorf(color); });

      impl_->entities.push_back(instance);
    }

    EntityModel DynamicScene::model_info(std::size_t model_id) const
    {
      const auto& model = impl_->entity_models[model_id];

      EntityModel model_info;
      model_info.texture = model.texture;
      model_info.type = model.type;
      return model_info;
    }

    std::size_t DynamicScene::model_count() const
    {
      return impl_->entity_models.size();
    }

    void DynamicScene::update_entity_positions()
    {
      for (auto& instance : impl_->entities)
      {
        instance.stored_position = instance.entity->position();
      }
    }

    DrawableEntity DynamicScene::entity_info(std::size_t instance_id) const
    {
      const auto& instance = impl_->entities[instance_id];
      const auto& model = impl_->entity_models[instance.model_id];

      auto entity = instance.entity;
      auto rotation = entity->rotation();
      auto new_position = entity->position();

      DrawableEntity drawable_entity;
      drawable_entity.entity = entity;
      drawable_entity.texture = model.texture;

      drawable_entity.colors.fill(1.0f);
      drawable_entity.colorizer_texture = nullptr;

      Rotation<double> deviation;
      auto frame_id = detail::calculate_frame_id(rotation, model.num_rotations, deviation);

      drawable_entity.texture_coords_offset = model.texture_coords[frame_id].first;
      drawable_entity.texture_coords_scale = model.texture_coords[frame_id].second;

      drawable_entity.model_transform = sf::Transform().
        translate(instance.stored_position.x, instance.stored_position.y).
        rotate(deviation.degrees()).
        scale(model.scale.x, model.scale.y);

      drawable_entity.new_model_transform = sf::Transform().
        translate(new_position.x, new_position.y).
        rotate(deviation.degrees()).
        scale(model.scale.x, model.scale.y);

      drawable_entity.colorizer_transform.
        combine(model.colorizer_transform).
        rotate(-rotation.degrees() + deviation.degrees());

      drawable_entity.level = entity->z_level();
      drawable_entity.shadow_offset = static_cast<float>(entity->hover_distance());
      drawable_entity.new_shadow_offset = drawable_entity.shadow_offset;

      if (instance.color_scheme)
      {
        auto scheme_id = instance.color_scheme->scheme_id;
        if (scheme_id < impl_->color_scheme_textures.size())
        {
          drawable_entity.colorizer_texture = &impl_->color_scheme_textures[scheme_id];

          std::uint32_t color_idx = 0;
          for (auto c : instance.color_scheme->colors)
          {
            drawable_entity.colors[color_idx++] = c.r;
            drawable_entity.colors[color_idx++] = c.g;
            drawable_entity.colors[color_idx++] = c.b;
          }
        }
      }

      return drawable_entity;
    }

    std::size_t DynamicScene::entity_count() const
    {
      return impl_->entities.size();
    }
  }
}


#pragma once

#include <cmath>
#include <chrono>
#include <iomanip>
#include <random>
#include <stdexcept>

#include <dtl/dtl.hpp>
#include <dtl/batchwise.hpp>
#include <dtl/math.hpp>
#include <dtl/mem.hpp>

#include <boost/math/common_factor.hpp>

#include "dtl/filter/blocked_bloomfilter/block_addressing_logic.hpp"
#include "blocked_cuckoofilter_logic.hpp"
#include "dtl/filter/blocked_bloomfilter/hash_family.hpp"

namespace dtl {

namespace internal {

//===----------------------------------------------------------------------===//
/// @see $u32& unroll_factor(u32, dtl::block_addressing, u32)
static constexpr u32 max_k = 16;

static
std::array<$u32, max_k * 5 /* different block sizes */ * 2 /* _addressing modes*/>
    unroll_factors_32 = {
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  1, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  2, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  4, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  8, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w = 16, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  1, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  2, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  4, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  8, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w = 16, a = magic
  };

static
std::array<$u32, max_k * 5 /* different block sizes */ * 2 /* _addressing modes*/>
    unroll_factors_64 = {
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  1, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  2, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  4, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  8, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w = 16, a = pow2
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  1, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  2, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  4, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w =  8, a = magic
    1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, // w = 16, a = magic
  };
//===----------------------------------------------------------------------===//

//===----------------------------------------------------------------------===//
// TODO remove
template<typename _word_src_t, typename _cuckoo_t>
struct blocked_cuckoofilter_api_adapter {

  using word_t = _word_src_t;

  using key_t = typename _cuckoo_t::key_t;
  using word_dst_t = typename _cuckoo_t::word_t;

  _cuckoo_t* filter;

  //===----------------------------------------------------------------------===//
  __forceinline__ __host__ __device__
  void
  insert(word_t* __restrict filter_data, const key_t& key) {
    return filter->insert(reinterpret_cast<word_dst_t*>(filter_data), key);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  __forceinline__ __host__
  void
  batch_insert(word_t* __restrict filter_data, const key_t* keys, const uint32_t key_cnt) {
    filter->batch_insert(reinterpret_cast<word_dst_t*>(filter_data),
                         keys, key_cnt);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  __forceinline__ __host__ __device__
  bool
  contains(const word_t* __restrict filter_data, const key_t& key) const {
    return filter->contains(reinterpret_cast<const word_dst_t*>(filter_data), key);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  __forceinline__  __host__
  uint64_t
  batch_contains(const word_t* __restrict filter_data,
                 const key_t* __restrict keys, const uint32_t key_cnt,
                 uint32_t* __restrict match_positions, const uint32_t match_offset) const {
    return filter->batch_contains(reinterpret_cast<const word_dst_t*>(filter_data),
                                  keys, key_cnt, match_positions, match_offset);
  };
  //===----------------------------------------------------------------------===//
};
//===----------------------------------------------------------------------===//

} // namespace internal

struct blocked_cuckoofilter {

  using key_t = $u32;
  using hash_value_t = $u32;
  using word_t = $u32;

// TODO ?
//  template<
//      typename key_t,
//      $u32 hash_fn_no
//  >
//  using hasher = dtl::hash::stat::mul32<key_t, hash_fn_no>;

  // The operations used for dynamic dispatching.
  enum class op_t {
    CONSTRUCT,  // constructs the filter logic
    BIND,       // assigns the function pointers of the filter API
    DESTRUCT,   // destructs the filter logic
  };

  static constexpr dtl::block_addressing power = dtl::block_addressing::POWER_OF_TWO;
  static constexpr dtl::block_addressing magic = dtl::block_addressing::MAGIC;

  template<u32 block_size_bytes, u32 tag_size_bits, u32 associativity, block_addressing addressing = power>
  using bcf = dtl::blocked_cuckoofilter_logic<block_size_bytes, tag_size_bits, associativity, addressing>;

  //===----------------------------------------------------------------------===//
  // Members
  //===----------------------------------------------------------------------===//
  /// The (desired) bit length of the filter.
  $u64 m;
  /// The (actual) bit length of the filter.
  $u64 m_actual;
  /// The block size in bytes.
  $u32 block_size_bytes;
  /// The size of a tag (aka signature) in bits.
  $u32 tag_size_bits;
  /// The number of slots per bucket.
  $u32 associativity;
  /// Pointer to the filter logic instance.
  void* instance = nullptr;
  // necessary because internally the word_t may differ
  void* adapter_instance = nullptr;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The API functions. (Function pointers to the actual implementation.)
  //===----------------------------------------------------------------------===//
  std::function<void(__restrict word_t* /*filter data*/, const key_t /*key*/)>
  insert;

  std::function<void(__restrict word_t* /*filter_data*/, const key_t* /*keys*/, u32 /*key_cnt*/)>
  batch_insert;

  std::function<$u1(const __restrict word_t* /*filter_data*/, const key_t /*key*/)>
  contains;

  std::function<$u64(const __restrict word_t* /*filter_ data*/,
                     const key_t* /*keys*/, u32 /*key_cnt*/,
                     $u32* /*match_positions*/, u32 /*match_offset*/)>
  batch_contains;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_cuckoofilter(const size_t m, u32 block_size_bytes = 64, u32 tag_size_bits = 16, u32 associativity = 4)
      : m(m), block_size_bytes(block_size_bytes), tag_size_bits(tag_size_bits), associativity(associativity) {

    // Construct the filter logic instance.
    dispatch(*this, op_t::CONSTRUCT);

    // Bind the API functions.
    dispatch(*this, op_t::BIND);

    // Check whether the constructed filter matches the given arguments.
    if (this->block_size_bytes != block_size_bytes
        || this->tag_size_bits != tag_size_bits
        || this->associativity != associativity) {
      dispatch(*this, op_t::DESTRUCT);
      throw std::invalid_argument("Invalid configuration: block_size_bytes=" + std::to_string(block_size_bytes)
                                  + ", tag_size_bits=" + std::to_string(tag_size_bits)
                                  + ", associativity=" + std::to_string(associativity));
    }

  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_cuckoofilter(blocked_cuckoofilter&& src)
      : m(src.m), m_actual(src.m_actual),
        block_size_bytes(src.block_size_bytes),
        tag_size_bits(src.tag_size_bits),
        associativity(src.associativity),
        instance(src.instance),
        insert(std::move(src.insert)),
        batch_insert(std::move(src.batch_insert)),
        contains(std::move(src.contains)),
        batch_contains(std::move(src.batch_contains)) {
    // Invalidate pointer in src
    src.instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  ~blocked_cuckoofilter() {
    // Destruct logic instance (if any).
    if (instance != nullptr) dispatch(*this, op_t::DESTRUCT);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_cuckoofilter&
  operator=(blocked_cuckoofilter&& src) {
    m = src.m;
    m_actual = src.m_actual;
    block_size_bytes = src.block_size_bytes;
    tag_size_bits = src.tag_size_bits;
    associativity = src.associativity;
    instance = src.instance;
    insert = std::move(src.insert);
    batch_insert = std::move(src.batch_insert);
    contains = std::move(src.contains);
    batch_contains = std::move(src.batch_contains);
    // invalidate pointers
    src.instance = nullptr;
    return *this;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Dynamic Dispatching
  //===----------------------------------------------------------------------===//
  //TODO make private
  static void
  dispatch(blocked_cuckoofilter& instance, op_t op) {
      switch (instance.block_size_bytes) {
        case   8: _s<  8>(instance, op); break;
        case  32: _s< 32>(instance, op); break;
        case  64: _s< 64>(instance, op); break;
        case 128: _s<128>(instance, op); break;
        case 256: _s<256>(instance, op); break;
        case 512: _s<512>(instance, op); break;
        default:
          throw std::invalid_argument("The given 'block_size_bytes' is not supported.");
      }
  };

  template<u32 b>
  static void
  _s(blocked_cuckoofilter& instance, op_t op) {
    switch (instance.tag_size_bits) {
      case  8:
        switch (instance.associativity) {
          case  4: _a<b,  8, 4>(instance, op); return;
          case  8: _a<b,  8, 8>(instance, op); return;
        }
        break;
      case 10:
        switch (instance.associativity) {
          case  6: _a<b, 10, 6>(instance, op); return;
        }
        break;
      case 12:
        switch (instance.associativity) {
          case  4: _a<b, 12, 4>(instance, op); return;
        }
        break;
      case 16:
        switch (instance.associativity) {
          case  2: _a<b, 16, 2>(instance, op); return;
          case  4: _a<b, 16, 4>(instance, op); return;
        }
        break;
    }
    throw std::invalid_argument("The given 'tag_size_bits/associativity' is not supported.");
  }

  template<u32 b, u32 t, u32 s>
  static void
  _a(blocked_cuckoofilter& instance, op_t op) {
    dtl::block_addressing addr = dtl::is_power_of_two(instance.m)
                                 ? dtl::block_addressing::POWER_OF_TWO
                                 : dtl::block_addressing::MAGIC;
    switch (addr) {
      case dtl::block_addressing::POWER_OF_TWO: _o<b, t, s, dtl::block_addressing::POWER_OF_TWO>(instance, op); break;
      case dtl::block_addressing::MAGIC:        _o<b, t, s, dtl::block_addressing::MAGIC>(instance, op);        break;
      case dtl::block_addressing::DYNAMIC:      /* must not happen */                                           break;
    }
  }

//  template<u32 b, u32 t, u32 s, dtl::block_addressing a>
//  static void
//  _u(blocked_cuckoofilter& instance, op_t op) {
//    switch (unroll_factor(k, a, w)) {
//      case  0: _o<b, t, s, a,  0>(instance, op); break;
//      case  1: _o<b, t, s, a,  1>(instance, op); break;
//      case  2: _o<b, t, s, a,  2>(instance, op); break;
//      case  4: _o<b, t, s, a,  4>(instance, op); break;
////      case  8: _o<w, s, k, a,  8>(instance, op); break;
//      default:
//        throw std::invalid_argument("The given 'unroll_factor' is not supported.");
//    }
//  }

  template<u32 b, u32 t, u32 s, dtl::block_addressing a>
  static void
  _o(blocked_cuckoofilter& instance, op_t op) {
    using _t = bcf<b, t, s, a>;
    switch (op) {
      case op_t::CONSTRUCT: instance._construct_logic<_t>();           break;
//      case op_t::BIND:      instance._bind_logic<_t, unroll_factor>(); break;
      case op_t::BIND:      instance._bind_logic<_t>(); break;
      case op_t::DESTRUCT:  instance._destruct_logic<_t>();            break;
    }
  };
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Constructs a blocked filter logic instance.
  template<typename bcf_t>
  void
  _construct_logic() {
    // Instantiate a filter logic.
    bcf_t* bcf = new bcf_t(m);
    instance = bcf;
    block_size_bytes = bcf_t::block_size_bytes;
    tag_size_bits = bcf_t::tag_size_bits;
    associativity = bcf_t::associativity;

    // Get the actual size of the filter.
    m_actual = bcf->get_length();

    using adapter_t = internal::blocked_cuckoofilter_api_adapter<word_t, bcf_t>;
    adapter_t* adapter = new adapter_t { bcf };
    adapter_instance = adapter;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Bind API functions to the (previously constructed) filter logic.
  template<typename bcf_t>
  void
  _bind_logic() {
    using namespace std::placeholders;
    using adapter_t = internal::blocked_cuckoofilter_api_adapter<word_t, bcf_t>;
//    auto* bcf = static_cast<bcf_t*>(instance);
    auto* bcf_adapter = static_cast<adapter_t*>(adapter_instance);

    // Bind the API functions.
    insert = std::bind(&adapter_t::insert, bcf_adapter, _1, _2);
    batch_insert = std::bind(&adapter_t::batch_insert, bcf_adapter, _1, _2, _3);
    contains = std::bind(&adapter_t::contains, bcf_adapter, _1, _2);
    batch_contains = std::bind(&adapter_t::batch_contains, bcf_adapter, _1, _2, _3, _4, _5);

//    // Bind the API functions.
//    insert = std::bind(&bcf_t::insert, bcf, _1, _2);
////    batch_insert = std::bind(&bcf_t::batch_insert, bcf, _1, _2, _3);
//    contains = std::bind(&bcf_t::contains, bcf, _1, _2);
//
//    // SIMD vector length (0 = run scalar code)
//    static constexpr u64 vector_len = dtl::simd::lane_count<key_t>; // * unroll_factor;
////    batch_contains = std::bind(&bcf_t::template batch_contains<vector_len>, bcf, _1, _2, _3, _4, _5);
//    batch_contains = std::bind(&bcf_t::batch_contains, bcf, _1, _2, _3, _4, _5);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Destructs the filter logic.
  template<typename bcf_t>
  void
  _destruct_logic() {
    using adapter_t = internal::blocked_cuckoofilter_api_adapter<word_t, bcf_t>;
    adapter_t* bcf_adapter = static_cast<adapter_t*>(adapter_instance);
    delete bcf_adapter;
    adapter_instance = nullptr;

    bcf_t* bcf = static_cast<bcf_t*>(instance);
    delete bcf;
    instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the block addressing mode.
  block_addressing get_addressing_mode() const {
    return dtl::is_power_of_two(m)
           ? block_addressing::POWER_OF_TWO
           : block_addressing::MAGIC;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (actual) size in bytes.
  std::size_t
  size_in_bytes() const noexcept {
    return (m_actual + 7) / 8;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (total) number of words.
  std::size_t
  size() const noexcept {
    constexpr u32 word_bitlength = sizeof(word_t) * 8;
    return (m_actual + (word_bitlength - 1)) / word_bitlength;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the name of the filter instance including the most
  /// important parameters (in JSON format).
  std::string
  name() const {
    return "{\"name\":\"blocked_cuckoo\", \"size\":" + std::to_string(size_in_bytes())
        + ",\"block_size\":" + std::to_string(block_size_bytes)
        + ",\"tag_bits\":" + std::to_string(tag_size_bits)
        + ",\"associativity\":" + std::to_string(associativity)
        + ",\"addr\":" + (get_addressing_mode() == dtl::block_addressing::POWER_OF_TWO ? "\"pow2\"" : "\"magic\"")
        + "}";
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Runs the calibration code. Results are stored in global variables.
  // TODO memoization in a global file / tool to calibrate
  static void
  calibrate(u64 filter_size_bits) __attribute__ ((noinline)) {
    std::cerr << "Running calibration..." << std::endl;
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<uint32_t> dis;

    static constexpr u32 data_size = 4u*1024*8;
    std::vector<key_t> random_data;
    random_data.reserve(data_size);
    for (std::size_t i = 0; i < data_size; i++) {
      random_data.push_back(dis(gen));
    }

    static const u32 max_unroll_factor = 4;
    for ($u32 w = 1; w <= 16; w *= 2) {
      for (auto addr_mode : {dtl::block_addressing::POWER_OF_TWO, dtl::block_addressing::MAGIC}) {
        for ($u32 k = 1; k <= 16; k++) {
          try {
            std::cerr << "w = " << std::setw(2) << w << ", "
                      << "addr = " << std::setw(5) << (addr_mode == block_addressing::POWER_OF_TWO ? "pow2" : "magic") << ", "
                      << "k = " <<  std::setw(2) << k << ": " << std::flush;

            $f64 cycles_per_lookup_min = std::numeric_limits<$f64>::max();
            $u32 u_min = 1;

            std::size_t match_count = 0;
            uint32_t match_pos[dtl::BATCH_SIZE];

            // baselines
            $f64 cycles_per_lookup_u0 = 0.0;
            $f64 cycles_per_lookup_u1 = 0.0;
            for ($u32 u = 0; u <= max_unroll_factor; u = (u == 0) ? 1 : u*2) {
              std::cerr << std::setw(2) << "u(" << std::to_string(u) + ") = "<< std::flush;
              unroll_factor(k, addr_mode, w) = u;
              $u32 sector_cnt = w;
              try {
                // with sectorization
                blocked_cuckoofilter bbf(filter_size_bits + 128 * static_cast<u32>(addr_mode), k, w, sector_cnt); // word_cnt = sector_cnt
              }
              catch (...) {
                // fall back to 1 sector
                sector_cnt = 1;
              }
              blocked_cuckoofilter bbf(filter_size_bits + 128 * static_cast<u32>(addr_mode), k, w, sector_cnt);
              std::vector<word_t, dtl::mem::numa_allocator<word_t>> filter_data(bbf.size(), 0);

              $u64 rep_cntr = 0;
              auto start = std::chrono::high_resolution_clock::now();
              auto tsc_start = _rdtsc();
              while (true) {
                std::chrono::duration<double> diff = std::chrono::high_resolution_clock::now() - start;
                if (diff.count() > 0.25) break;
                dtl::batch_wise(random_data.begin(), random_data.end(), [&](const auto batch_begin, const auto batch_end) {
                  match_count += bbf.batch_contains(&filter_data[0], &batch_begin[0], batch_end - batch_begin, match_pos, 0);
                });
                rep_cntr++;
              }
              auto tsc_end = _rdtsc();
              auto cycles_per_lookup = (tsc_end - tsc_start) / (data_size * rep_cntr * 1.0);
              if (u == 0) cycles_per_lookup_u0 = cycles_per_lookup;
              if (u == 1) cycles_per_lookup_u1 = cycles_per_lookup;
              std::cerr << std::setprecision(3) << std::setw(4) << std::right << cycles_per_lookup << ", ";
              if (cycles_per_lookup < cycles_per_lookup_min) {
                cycles_per_lookup_min = cycles_per_lookup;
                u_min = u;
              }
            }
            unroll_factor(k, addr_mode, w) = u_min;
            std::cerr << " picked u = " << unroll_factor(k, addr_mode, w)
                      << ", speedup over u(0) = " << std::setprecision(3) << std::setw(4) << std::right << (cycles_per_lookup_u0 / cycles_per_lookup_min)
                      << ", speedup over u(1) = " << std::setprecision(3) << std::setw(4) << std::right << (cycles_per_lookup_u1 / cycles_per_lookup_min)
                      << " (chksum: " << match_count << ")" << std::endl;

          } catch (...) {
            std::cerr<< " -> Failed to calibrate for k = " << k << "." << std::endl;
          }
        }
      }
    }
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the SIMD unrolling factor for the given k and addressing mode.
  /// Note: unrolling by 0 means -> scalar code (no SIMD)
  static $u32&
  unroll_factor(u32 k, dtl::block_addressing addr_mode, u32 word_cnt_per_block) {
    auto& unroll_factors = sizeof(word_t) == 8
                           ? internal::unroll_factors_64
                           : internal::unroll_factors_32;
    return unroll_factors[
        internal::max_k * dtl::log_2(word_cnt_per_block)
        + (k - 1)
        + (static_cast<u32>(addr_mode) * internal::max_k * 5)
    ];
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Force unroll factor for all implementations (used for benchmarking)
  static void
  force_unroll_factor(u32 u) {
    for ($u32 w = 1; w <= 16; w *= 2) {
      for (auto addr_mode : {dtl::block_addressing::POWER_OF_TWO, dtl::block_addressing::MAGIC}) {
        for ($u32 k = 1; k <= 16; k++) {
          unroll_factor(k, addr_mode, w) = u;
        }
      }
    }
  }
  //===----------------------------------------------------------------------===//

};

} // namespace dtl


#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/graph_coloring.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/lac/trilinos_precondition.h>
#include <deal.II/lac/trilinos_solver.h>
#include <deal.II/lac/trilinos_vector.h>
#include <deal.II/numerics/vector_tools.h>
// system includes ------------------------------------------------------------
#include <EpetraExt_HDF5.h>
#include <algorithm>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <boost/static_assert.hpp>
#include <cmath>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
// own includes ---------------------------------------------------------------
#include "bte_config.h"
#include "app/app.hpp"
#include "aux/debug_output.hpp"
#include "aux/message.hpp"
#include "collision_tensor/collision_tensor.hpp"
#include "export/epetra_row_matrix.hpp"
//#include "collision_tensor/collision_tensor_factory.hpp"
#include "collision_tensor/collision_tensor_operator.hpp"
#include "export/export_dh.hpp"
#include "grid/dof_mapper_periodic_distributed.hpp"
#include "grid/grid_handler_periodic.hpp"
#include "init/import/load_coefficients.hpp"
#include "matrix/assembly/assembly.hpp"
#include "matrix/assembly/velocity_var_form.hpp"
#include "matrix/dofs/dofindex_sets.hpp"
#include "matrix/dofs/periodic_utility.hpp"
#include "matrix/system_matrix_handler.hpp"
#include "method/method.hpp"
#include "post_processing/xdmf_exporter.hpp"
#include "solver/aztecOO_condest.hpp"
#include "solver/solver_handler.hpp"
#include "spectral/basis/indexer.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

using namespace std;
using namespace dealii;
using namespace boltzmann;
namespace bf = boost::filesystem;
namespace po = boost::program_options;

typedef SpectralBasisFactoryKS basis_factory_t;

const int dim = 2;
const double PI = dealii::numbers::PI;

// define the method
typedef Method<METHOD::MODLEASTSQUARES> method_t;
typedef App<dim> app_t;

typedef TrilinosWrappers::MPI::Vector vector_t;
// typedef TrilinosWrappers::Vector vector_local_t;
typedef TrilinosWrappers::SparseMatrix matrix_t;

int
main(int argc, char* argv[])
{
  boltzmann::Timer<> timer;
  int nthreads;
  // --------------------------------------------------
  // program options (read initial conditions from file)
  string scratch_dir;
  po::options_description options("options");
  options.add_options()("help", "produce help message")(
      "export-matrices", "export matrices to hdf5 file")("export-dofs", "export dofs and exit")(
      "petrov-galerkin", "use Galerkin disc. for scattering")(
      "threads,t", po::value<int>(&nthreads)->default_value(1), "number of threads")(
      "init,i", po::value<string>()->default_value(""), "initial distribution, dset='coeffs'")(
      "condest", "estimate condition number");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);
  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, nthreads);
  const unsigned int process_id = Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  const unsigned int nprocs = Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD);

  ConditionalOStream pcout(std::cout, process_id == 0);
  pcout << "using " << nprocs << " MPI x " << nthreads << " OMP threads\n";
  pcout << "executable: " << argv[0] << endl << "periodic boundary conditions" << endl;

  std::string version_id = GIT_SHA1;
  pcout << "VersionID: " << version_id << "@" << GIT_BNAME << std::endl;

  // ----------------------------------------
  // yaml config
  if (!boost::filesystem::is_regular_file("config.yaml")) {
    pcout << "config file not found\n";
    return 1;
  }
  YAML::Node config = YAML::LoadFile("config.yaml");

  // make sure convergence messages etc. ware written only by proc 0
  if (process_id != 0) dealii::deallog.depth_console(0);
  pcout << "\n----------------------------------------\n";
  pcout << config << endl;
  pcout << "\n----------------------------------------\n";

  const size_t nK = config["SpectralBasis"]["deg"].as<size_t>();
  const double beta = 2;
  const double dt = config["TimeStepping"]["dt"].as<double>();
  // output frequency
  const size_t ntsteps = config["TimeStepping"]["N"].as<size_t>();

  // ----------------------------------------
  // MESH
  timer.start();
  GridHandlerPeriodic<2> grid_handler;
  grid_handler.init(config, nprocs);
  print_timer(timer.stop(), "initialize mesh", pcout);
  const auto& dof_handler = grid_handler.dofhandler();
  if (process_id == 0) export_dh(dof_handler);
  // ----------------------------------------------------------------------
  typedef basis_factory_t::basis_type basis_type;
  basis_type spectral_basis;
  basis_type spectral_test_basis;

  if (bf::exists("spectral_basis.desc")) {
    // read from file if possible
    // pcout << "reading basis from file" << std::endl;
    basis_factory_t::create(spectral_basis, "spectral_basis.desc");
  } else {
    bool sorted = true;
    basis_factory_t::create(spectral_basis, nK, nK, beta, sorted);
    if (process_id == 0)
      basis_factory_t::write_basis_descriptor(spectral_basis, "spectral_basis.desc");
  }

  if (bf::exists("spectral_basis_test.desc")) {
    basis_factory_t::create(spectral_test_basis, "spectral_basis_test.desc");
  } else {
    bool sorted = true;
    if (vm.count("petrov-galerkin"))
      basis_factory_t::create_test(spectral_test_basis, nK, nK, beta, sorted);
    else
      basis_factory_t::create(spectral_test_basis, nK, nK, beta, sorted);

    if (process_id == 0)
      basis_factory_t::write_basis_descriptor(spectral_test_basis, "spectral_basis_test.desc");
  }

  if (vm.count("export-dofs")) {
    pcout << "L: " << dof_handler.n_dofs() << "\n N: " << spectral_basis.n_dofs()
          << "\n L x N: " << dof_handler.n_dofs() * spectral_basis.n_dofs() << "\n";

    pcout << "\nvertex2dofix.dat/ dof.desc written to disk. Exit.\n";
    return 0;
  }

  // ----------------------------------------------------------------------------------------------------
  // DoFs
  const size_t n_phys_dofs = dof_handler.n_dofs();
  const size_t n_velo_dofs = spectral_basis.n_dofs();
  const size_t n_dofs = spectral_basis.n_dofs() * dof_handler.n_dofs();

  pcout << "#vertices on grid"
        << "\t" << n_phys_dofs << endl
        << "dim(V)"
        << "\t" << n_velo_dofs << endl;
  // regular indexer on full grid (used for post_processing and output)
  Indexer<> global_indexer(n_phys_dofs, n_velo_dofs);
  // indexer used for DoF enumeration
  typedef DoFMapperPeriodicDistributed<> dof_mapper_t;
  dof_mapper_t dof_mapper;
  dof_mapper.init(dof_handler, nprocs);
  IndexSet owned_phys_dofs = dof_mapper.owned_dofs(process_id);
  IndexSet relevant_phys_dofs = dof_mapper.relevant_dofs(process_id);

  DoFIndexSetsPeriodic index_sets(owned_phys_dofs, relevant_phys_dofs, n_velo_dofs, process_id);

  IndexSet owned_dofs = index_sets.locally_owned_dofs(process_id);
  IndexSet relevant_dofs = index_sets.locally_relevant_dofs();
  Indexer<dof_mapper_t> indexer(dof_mapper, n_phys_dofs, n_velo_dofs);
  pcout << "#DoFs = " << indexer.n_dofs() << endl;
  /// DoFIndexSets provides index_maps on distributed (unrestricted) grid
  // ----------------------------------------------------------------------
  // Velocity matrix entries
  VelocityVarForm<2> velocity_var_form;
  velocity_var_form.init(spectral_basis, beta);
  // --------------------------------------------------------------------------------------------------
  // Scattering
  bool has_scattering = (bool) config["Scattering"];
  CollisionTensorOperatorBase* QO = NULL;
  double kn = 1;  // Knudsen-number
  if (has_scattering) {
    std::string tensor_fname = config["Scattering"]["file"].as<std::string>().c_str();
    if (config["Scattering"]["Galerkin"] && !vm.count("petrov-galerkin")) {
      pcout << "CollisionTensor Galerkin\n";
      QO = new CollisionTensorOperatorG(
          dof_handler, spectral_basis, index_sets.locally_owned_dofs(process_id));
    } else if (config["Scattering"]["Petrov-Galerkin"] && vm.count("petrov-galerkin")) {
      pcout << "CollisionTensor Petrov-Galerkin\n";
      QO = new CollisionTensorOperatorPG(
          dof_handler, spectral_basis, index_sets.locally_owned_dofs(process_id));
    } else {
      // default
      pcout << "CollisionTensor Galerkin (DENSE)\n";
      QO = new CollisionTensorOperatorDense<>(
          dof_handler, spectral_basis, index_sets.locally_owned_dofs(process_id));
    }
    timer.start();
    QO->load_tensor(tensor_fname);
    print_timer(timer.stop(), "load collision_tensor", pcout);

    if (config["Scattering"]["kn"]) {
      kn = config["Scattering"]["kn"].as<double>();
    }
  }

  SystemMatrixHandler<method_t, app_t> system_matrix_handler(
      dof_handler, spectral_basis, indexer, index_sets, dt);

  const auto& A = system_matrix_handler.get_lhs();
  const auto& M = system_matrix_handler.get_rhs();

#ifdef HAVE_EPETRAEXT_HDF5
  if (vm.count("export-matrices")) {
    pcout << "Export matrices to hdf5\n";
    ExportEpetraHDF5 matrix_exporter(A.trilinos_matrix().Comm());
    matrix_exporter.write("A", A.trilinos_matrix());
    matrix_exporter.write("M", M.trilinos_matrix());
  }
#endif

  // estimate condition number
  if (vm.count("condest")) {
    int maxiters = 100;
    // estimate condest from BC
    AztecOOCondest condestCG;
    condestCG.initialize(A, "GMRES");
    double condCG = condestCG.compute(maxiters);
    pcout << "estimated condition number (GMRES): " << condCG << endl;
  }

  // ----------------------------------------------------------------------
  // Initial conditions
  // create f0 on all DoFs
  vector<double> f0_full_grid(n_velo_dofs * n_phys_dofs);
  pcout << "loading initial-distribution from file: " << vm["init"].as<string>() << endl;
  load_coefficients(f0_full_grid, vm["init"].as<string>(), dof_handler, global_indexer);
  // convert between periodic and non-periodic solution on phase space grid
  vector_t mu(owned_dofs);
  vector_t mu_ghosted(owned_dofs, relevant_dofs);
  // ----------------------------------------------------------------------
  // Output
  GridHelper grid_helper(n_velo_dofs, n_phys_dofs);
  grid_helper.to_restricted_grid(mu, f0_full_grid, global_indexer, indexer, owned_phys_dofs);

  /*
   *   hint: DoFIndexSets contains full grid indices
   */
  DoFIndexSets dof_map(nprocs);
  dof_map.init(dof_handler, n_velo_dofs);
  IndexSet ghosts = dof_map.locally_relevant_dofs();
  ghosts.subtract_set(dof_map.locally_owned_dofs(process_id));

  vector_t mu_f(dof_map.locally_owned_dofs(process_id));
  mu_ghosted = mu;
  auto dofs = dof_map.locally_owned_phys_dofs(process_id);
  unsigned int n_vtk = config["TimeStepping"]["export_vtk"]
                           ? config["TimeStepping"]["export_vtk"].as<unsigned int>()
                           : 1;
  XDMFH5Exporter xdmf_exporter(
      dof_handler, spectral_basis, dof_map.locally_owned_phys_dofs(process_id), 1, n_vtk);

  grid_helper.to_full_grid(mu_f, mu_ghosted, global_indexer, indexer, dofs);
  xdmf_exporter(mu_f, 0);
  // mu_f_ghosted = mu_f;
  //  output.run(mu_f_ghosted, 0, 0, global_indexer);
  // ----------------------------------------------------------------------------------------------------
  // Timestepping
  pcout << "\n\n-------------------- TIME STEPPING --------------------\n\n";
  vector_t rhs(owned_dofs);
  vector_t sc(owned_dofs);
  double current_time = 0;
  // dump full solution vector / export paraview data every xx timestep
  unsigned int n_dump =
      config["TimeStepping"]["dump"] ? config["TimeStepping"]["dump"].as<unsigned int>() : 0;

  SolverHandler solver_handler;
  timer.start();
  solver_handler.init(config, A);
  print_timer(timer.stop(), "Preconditioner", pcout);
  auto& solver = solver_handler.get_solver();
  auto& P = solver_handler.get_preconditioner();

#ifdef HAVE_EPETRAEXT_HDF5
  // write full solution to disk
  const auto& trilinos_vector = mu_f.trilinos_vector();
  EpetraExt::HDF5 epetra_exporter(trilinos_vector.Comm());
  epetra_exporter.Create("solution_vector" + boost::lexical_cast<string>(0) + ".h5");
  epetra_exporter.Write(boost::lexical_cast<string>(0), trilinos_vector);
  epetra_exporter.Flush();
  epetra_exporter.Close();
#endif

  for (unsigned int i = 1; i <= ntsteps; ++i) {
    timer.start();
    M.vmult(rhs, mu);
    print_timer(timer.stop(), "MV-product", pcout);
    timer.start();
    solver.solve(A, mu, rhs, P);
    auto ctr = solver.control();
    pcout << "Solver::GMRES " << ctr.last_step() << "\t" << ctr.last_value() << "\n";
    print_timer(timer.stop(), "timestep transport", pcout);

    if (has_scattering) {
      timer.start();
      QO->apply(mu, dt / kn);
      print_timer(timer.stop(), "timestep scattering", pcout);
    }

    current_time += dt;
    mu_ghosted = mu;
    grid_helper.to_full_grid(mu_f, mu_ghosted, global_indexer, indexer, dofs);
    xdmf_exporter(mu_f, current_time);

#ifdef HAVE_EPETRAEXT_HDF5
    if ((n_dump && (i % n_dump == 0)) || i == ntsteps) {
      timer.start();
      const auto& trilinos_vector = mu_f.trilinos_vector();
      EpetraExt::HDF5 epetra_exporter2(trilinos_vector.Comm());
      epetra_exporter2.Create("solution_vector" + boost::lexical_cast<string>(i) + ".h5");
      epetra_exporter2.Write(boost::lexical_cast<string>(i), trilinos_vector);
      epetra_exporter2.Flush();
      epetra_exporter2.Close();
      print_timer(timer.stop(), "EpetraExt IO", pcout);
    }
#endif
  }

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_REDUCTION_FUNCTIONS_CONTAINER_SUM_HPP_INCLUDED
#define NT2_REDUCTION_FUNCTIONS_CONTAINER_SUM_HPP_INCLUDED

#include <nt2/reduction/functions/sum.hpp>
#include <nt2/core/container/dsl/reduction.hpp>
#include <boost/simd/reduction/functions/sum.hpp>

#include <nt2/include/functions/plus.hpp>
#include <nt2/include/constants/zero.hpp>

namespace nt2 { namespace ext
{
  template<class Domain, class Expr>
  struct  size_of<boost::simd::tag::sum_,Domain,1,Expr>
        : meta::reduction_size<Expr,1>
  {};

  template<class Domain, class Expr>
  struct  size_of<boost::simd::tag::sum_,Domain,2,Expr>
        : meta::reduction_size<Expr,2>
  {};

  template<class Domain, int N, class Expr>
  struct  value_type<boost::simd::tag::sum_,Domain,N,Expr>
        : meta::reduction_value<boost::simd::tag::sum_,Expr>
  {};
} }

#endif


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "filtered_read_fraction.hpp"

#include <iterator>
#include <algorithm>

#include <boost/variant.hpp>

namespace octopus { namespace csr {

const std::string FilteredReadFraction::name_ = "FRF";

FilteredReadFraction::FilteredReadFraction(bool aggregate_samples)
: calling_depth_ {false, aggregate_samples}
, filtering_depth_ {true, aggregate_samples}
{}

std::unique_ptr<Measure> FilteredReadFraction::do_clone() const
{
    return std::make_unique<FilteredReadFraction>(*this);
}

Measure::ValueType FilteredReadFraction::get_value_type() const
{
    return double {};
}

Measure::ResultType FilteredReadFraction::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    if (filtering_depth_.cardinality() == Measure::ResultCardinality::samples) {
        const auto raw_depth = boost::get<Array<ValueType>>(filtering_depth_.evaluate(call, facets));
        const auto filtered_depth   = boost::get<Array<ValueType>>(calling_depth_.evaluate(call, facets));
        assert(raw_depth.size() == filtered_depth.size());
        Array<ValueType> result(raw_depth.size());
        const auto filtered_read_fraction_helper = [] (const auto raw_depth, const auto filtered_depth) {
            return raw_depth > 0 ? 1.0 - (static_cast<double>(filtered_depth) / raw_depth) : 0.0;
        };
        const auto filtered_read_fraction = [&] (const auto raw_depth, const auto filtered_depth) -> ValueType {
            return filtered_read_fraction_helper(boost::get<std::size_t>(raw_depth), boost::get<std::size_t>(filtered_depth));
        };
        std::transform(std::cbegin(raw_depth), std::cend(raw_depth), std::cbegin(filtered_depth),
                       std::begin(result), filtered_read_fraction);
        return result;
    } else {
        const auto raw_depth = boost::get<std::size_t>(boost::get<ValueType>(filtering_depth_.evaluate(call, facets)));
        ValueType result {0};
        if (raw_depth > 0) {
            auto filtered_depth = boost::get<std::size_t>(boost::get<ValueType>(calling_depth_.evaluate(call, facets)));
            result = 1.0 - (static_cast<double>(filtered_depth) / raw_depth);
        }
        return result;
    }
}

Measure::ResultCardinality FilteredReadFraction::do_cardinality() const noexcept
{
    return filtering_depth_.cardinality();
}

const std::string& FilteredReadFraction::do_name() const
{
    return name_;
}

std::string FilteredReadFraction::do_describe() const
{
    return "Fraction of reads filtered for calling";
}

std::vector<std::string> FilteredReadFraction::do_requirements() const
{
    return filtering_depth_.requirements();
}

bool FilteredReadFraction::is_equal(const Measure& other) const noexcept
{
    return calling_depth_ == static_cast<const FilteredReadFraction&>(other).calling_depth_;
}

} // namespace csr
} // namespace octopus


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

  Defines the as_simd metafunction for MIC like extensions

  @copyright 2012 - 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_MIC_AS_SIMD_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_MIC_AS_SIMD_HPP_INCLUDED

#include <boost/simd/arch/common/simd/as_simd.hpp>
#include <boost/dispatch/meta/is_natural.hpp>
#include <type_traits>

namespace boost { namespace simd
{
  template<typename T> struct logical;

  namespace ext
  {
    template<> struct as_simd<float, boost::simd::mic_>
    {
      using type = __m512;
    };

    template<> struct as_simd<double, boost::simd::mic_>
    {
      using type = __m512d;
    };

    template<typename T>
    struct as_simd< T, boost::simd::mic_
                  , typename std::enable_if <   boost::dispatch::is_natural<T>::value
                                            &&  (sizeof(T) == 4 || sizeof(T) == 8)
                                            >::type
                  >
    {
      using type = __m512i;
    };

    template<typename T>
    struct as_simd<logical<T>, boost::simd::mic_, typename std::enable_if< sizeof(T) == 4 >::type>
    {
      using type = __mmask16;
    };

    template<typename T>
    struct as_simd<logical<T>, boost::simd::mic_, typename std::enable_if< sizeof(T) == 8 >::type>
    {
      using type = __mmask8;
    };
  }
} }

#endif