#include "cpu/graph_proc.h"

#include <set>
#include <vector>
#include <numeric> //std::iota
#include <algorithm>
#include <random>
#include <iostream>

#include <Eigen/Dense>

using std::vector;

namespace graph_proc {

    py::array_t<bool> erode_mesh(const py::array_t<float>& vertexPositions, const py::array_t<int>& faceIndices, int nIterations, int minNeighbors) {
        int nVertices = vertexPositions.shape(0);
        int nFaces = faceIndices.shape(0);

        // Init output
        py::array_t<bool> nonErodedVertices = py::array_t<bool>({ nVertices, 1 });
        std::vector<bool> nonErodedVerticesVec(nVertices, false);

        // Init list of eroded face indices with original list
        std::vector<Eigen::Vector3i> erodedFaceIndicesVec;
        erodedFaceIndicesVec.reserve(nFaces);
        for (int FaceIdx = 0; FaceIdx < nFaces; ++FaceIdx) {
            Eigen::Vector3i face(*faceIndices.data(FaceIdx, 0), *faceIndices.data(FaceIdx, 1), *faceIndices.data(FaceIdx, 2));
            erodedFaceIndicesVec.push_back(face);
        }

        // Erode mesh for a total of nIterations
        for (int i = 0; i < nIterations; i++) {
            nFaces = erodedFaceIndicesVec.size();

            // We compute the number of neighboring vertices for each vertex.
            vector<int> numNeighbors(nVertices, 0);
            for (int i = 0; i < nFaces; i++) {
                const auto& face = erodedFaceIndicesVec[i];
                numNeighbors[face[0]] += 1;
                numNeighbors[face[1]] += 1;
                numNeighbors[face[2]] += 1;
            }

            std::vector<Eigen::Vector3i> tmp;
            tmp.reserve(nFaces);

            for (int i = 0; i < nFaces; i++) {
                const auto& face = erodedFaceIndicesVec[i];
                if (numNeighbors[face[0]] >= minNeighbors && numNeighbors[face[1]] >= minNeighbors && numNeighbors[face[2]] >= minNeighbors) {
                    tmp.push_back(face);
                }
            }

            // We kill the faces with border vertices.
            erodedFaceIndicesVec.clear();
            erodedFaceIndicesVec = std::move(tmp);
        }

        // Mark non isolated vertices as not eroded.
        nFaces = erodedFaceIndicesVec.size();

        for (int i = 0; i < nFaces; i++) {
            const auto& face = erodedFaceIndicesVec[i];
            nonErodedVerticesVec[face[0]] = true;
            nonErodedVerticesVec[face[1]] = true;
            nonErodedVerticesVec[face[2]] = true;
        }

        // Store into python array
        for (int i = 0; i < nVertices; i++) {
            *nonErodedVertices.mutable_data(i, 0) = nonErodedVerticesVec[i];
        }

        return nonErodedVertices;
    }

    int sample_nodes(
        const py::array_t<float>& vertexPositions, const py::array_t<bool>& nonErodedVertices,
        py::array_t<float>& nodePositions, py::array_t<int>& nodeIndices,
        float nodeCoverage,
        const bool useOnlyNonErodedIndices=true,
        const bool randomShuffle=true
    ) {
        // assert(vertexPositions.ndim() == 2);

        float nodeCoverage2 = nodeCoverage * nodeCoverage;
        int nVertices = vertexPositions.shape(0);
        // assert(vertexPositions.shape(1) == 3);
        // assert(nodePositions.shape(0) == nVertices);
        // assert(nodePositions.shape(1) == 3);
        // assert(nodeIndices.shape(0) == nVertices);
        // assert(nodeIndices.shape(1) == 1);

        nodePositions.resize({ nVertices, 3 }, false);
        nodeIndices.resize({ nVertices, 1 }, false);

        // create list of shuffled indices
        std::vector<int> shuffledVertices(nVertices);
        std::iota(std::begin(shuffledVertices), std::end(shuffledVertices), 0);

        if (randomShuffle) {
            std::default_random_engine re{std::random_device{}()};
            std::shuffle(std::begin(shuffledVertices), std::end(shuffledVertices), re);
        }

        std::vector<Eigen::Vector3f> nodePositionsVec;
        for (int vertexIdx : shuffledVertices) {
        // for (int vertexIdx = 0; vertexIdx < nVertices; ++vertexIdx) {
            Eigen::Vector3f point(*vertexPositions.data(vertexIdx, 0), *vertexPositions.data(vertexIdx, 1), *vertexPositions.data(vertexIdx, 2));

            if (useOnlyNonErodedIndices && !(*nonErodedVertices.data(vertexIdx))) {
                continue;
            }

            bool bIsNode = true;
            for (int nodeIdx = 0; nodeIdx < nodePositionsVec.size(); ++nodeIdx) {
                if ((point - nodePositionsVec[nodeIdx]).squaredNorm() <= nodeCoverage2) {
                    bIsNode = false;
                    break;
                }
            }

            if (bIsNode) {
                nodePositionsVec.push_back(point);
                int newNodeIdx = nodePositionsVec.size() - 1;
                *nodePositions.mutable_data(newNodeIdx, 0) = point.x();
                *nodePositions.mutable_data(newNodeIdx, 1) = point.y();
                *nodePositions.mutable_data(newNodeIdx, 2) = point.z();
                *nodeIndices.mutable_data(newNodeIdx, 0) = vertexIdx;
            }
        }

        return nodePositionsVec.size();
    }

    /**
     * Custom comparison operator for geodesic priority queue.
     */
    struct CustomCompare {
        bool operator()(const std::pair<int, float>& left, const std::pair<int, float>& right) {
            return left.second > right.second;
        }
    };

    inline float compute_anchor_weight(const Eigen::Vector3f& pointPosition, const Eigen::Vector3f& nodePosition, float nodeCoverage) {
        return std::exp(-(nodePosition - pointPosition).squaredNorm() / (2.f * nodeCoverage * nodeCoverage));
    }

    inline float compute_anchor_weight(float dist, float nodeCoverage) {
        return std::exp(- (dist * dist) / (2.f * nodeCoverage * nodeCoverage));
    }

    void compute_edges_geodesic(
		const py::array_t<float>& vertexPositions,
		const py::array_t<bool>& validVertices,
		const py::array_t<int>& faceIndices,
		const py::array_t<int>& nodeIndices,
		const int nMaxNeighbors, const float nodeCoverage,
        py::array_t<int>& graphEdges,
        py::array_t<float>& graphEdgesWeights,
        py::array_t<float>& graphEdgesDistances,
        py::array_t<float>& nodeToVertexDistances,
        const bool allow_only_valid_vertices,
        const bool enforce_total_num_neighbors
	) {
		int nVertices = vertexPositions.shape(0);
		int nFaces = faceIndices.shape(0);
        int nNodes = nodeIndices.shape(0);

        float maxInfluence = 2.f * nodeCoverage;

        // Preprocess vertex neighbors.
		vector<std::set<int>> vertexNeighbors(nVertices);
        for (int faceIdx = 0; faceIdx < nFaces; faceIdx++) {
            for (int j = 0; j < 3; j++) {
                int v_idx = *faceIndices.data(faceIdx, j);

                for (int k = 0; k < 3; k++) {
                    int n_idx = *faceIndices.data(faceIdx, k);

                    if (v_idx == n_idx) continue;
                    vertexNeighbors[v_idx].insert(n_idx);
                }
            }
        }

		// Compute inverse vertex -> node relationship.
		vector<int> mapVertexToNode(nVertices, -1);

		for (int nodeId = 0; nodeId < nNodes; nodeId++) {
			int vertexIdx = *nodeIndices.data(nodeId);
			if (vertexIdx >= 0) {
				mapVertexToNode[vertexIdx] = nodeId;
			}
		}

        // #pragma omp parallel for
		for (int nodeId = 0; nodeId < nNodes; nodeId++) {
			// vertex queue
            std::priority_queue<
				std::pair<int, float>,
				vector<std::pair<int, float>>,
				CustomCompare
			> nextVerticesWithIds;

			std::set<int> visitedVertices;

			// Add node vertex as the first vertex to be visited.
			int nodeVertexIdx = *nodeIndices.data(nodeId);
			if (nodeVertexIdx < 0) continue;
			nextVerticesWithIds.push(std::make_pair(nodeVertexIdx, 0.f));

			// Traverse all neighbors in the monotonically increasing order.
            vector<int>   neighborNodeIds;
            vector<float> neighborNodeWeights;
            vector<float> neighborNodeDistances;
			while (!nextVerticesWithIds.empty()) {
				auto nextVertex = nextVerticesWithIds.top();
				nextVerticesWithIds.pop();

				int nextVertexIdx = nextVertex.first;
				float nextVertexDist = nextVertex.second;

				// We skip the vertex, if it was already visited before.
				if (visitedVertices.find(nextVertexIdx) != visitedVertices.end()) continue;

                if (allow_only_valid_vertices && !*validVertices.data(nextVertexIdx)) {
                    std::cout << "compute_edges_geodesic:: ufff... we shouldn't be checking out this vertex" << std::endl;
                    exit(0);
                }

				// We check if the vertex is a node.
				int nextNodeId = mapVertexToNode[nextVertexIdx];
				if (nextNodeId >= 0 && nextNodeId != nodeId) {
                    neighborNodeIds.push_back(nextNodeId);
                    neighborNodeWeights.push_back(compute_anchor_weight(nextVertexDist, nodeCoverage));
                    neighborNodeDistances.push_back(nextVertexDist);
                    if (neighborNodeIds.size() >= nMaxNeighbors) break;
				}

				// Note down the node-vertex distance.
				*nodeToVertexDistances.mutable_data(nodeId, nextVertexIdx) = nextVertexDist;

				// We visit the vertex, and check all his neighbors.
				// We add only valid vertices under a certain distance
				visitedVertices.insert(nextVertexIdx);
				Eigen::Vector3f nextVertexPos(*vertexPositions.data(nextVertexIdx, 0), *vertexPositions.data(nextVertexIdx, 1), *vertexPositions.data(nextVertexIdx, 2));

				const auto& nextNeighbors = vertexNeighbors[nextVertexIdx];
				for (int neighborIdx : nextNeighbors) {

                    bool is_valid_vertex = *validVertices.data(neighborIdx);
                    if (allow_only_valid_vertices && !is_valid_vertex) {
                        continue;
                    }

					Eigen::Vector3f neighborVertexPos(*vertexPositions.data(neighborIdx, 0), *vertexPositions.data(neighborIdx, 1), *vertexPositions.data(neighborIdx, 2));
					float dist = nextVertexDist + (nextVertexPos - neighborVertexPos).norm();

                    if (enforce_total_num_neighbors) {
						nextVerticesWithIds.push(std::make_pair(neighborIdx, dist));
                    }
					else {
                        // std::cout << dist << " " << maxInfluence << std::endl;
                        if (dist <= maxInfluence) {
						    nextVerticesWithIds.push(std::make_pair(neighborIdx, dist));
					    }
                    }
				}
			}

            // Store the nearest neighbors.
            int nNeighbors = neighborNodeIds.size();

            float weightSum = 0.f;
            for (int i = 0; i < nNeighbors; i++) {
                *graphEdges.mutable_data(nodeId, i) = neighborNodeIds[i];
                weightSum += neighborNodeWeights[i];
            }

            // Normalize weights
            if (weightSum > 0) {
                for (int i = 0; i < nNeighbors; i++) {
                    *graphEdgesWeights.mutable_data(nodeId, i) = neighborNodeWeights[i] / weightSum;
                }
            }
            else if (nNeighbors > 0) {
                for (int i = 0; i < nNeighbors; i++) {
                    *graphEdgesWeights.mutable_data(nodeId, i) = neighborNodeWeights[i] / nNeighbors;
                }
            }

            // Store edge distance.
            for (int i = 0; i < nNeighbors; i++) {
                *graphEdgesDistances.mutable_data(nodeId, i) = neighborNodeDistances[i];
            }
		}
    }

    py::array_t<int> compute_edges_euclidean(const py::array_t<float>& nodePositions, int nMaxNeighbors) {
        int nNodes = nodePositions.shape(0);

        py::array_t<int> graphEdges = py::array_t<int>({ nNodes, nMaxNeighbors });

        // Find nearest Euclidean neighbors for each node.
        for (int nodeId = 0; nodeId < nNodes; nodeId++) {
            Eigen::Vector3f nodePos(*nodePositions.data(nodeId, 0), *nodePositions.data(nodeId, 1), *nodePositions.data(nodeId, 2));

            // Keep only the k nearest Euclidean neighbors.
            std::list<std::pair<int, float>> nearestNodesWithSquaredDistances;

            for (int neighborId = 0; neighborId < nNodes; neighborId++) {
                if (neighborId == nodeId) continue;

                Eigen::Vector3f neighborPos(*nodePositions.data(neighborId, 0), *nodePositions.data(neighborId, 1), *nodePositions.data(neighborId, 2));

                float distance2 = (nodePos - neighborPos).squaredNorm();
                bool bInserted = false;
                for (auto it = nearestNodesWithSquaredDistances.begin(); it != nearestNodesWithSquaredDistances.end(); ++it) {
                    // We insert the element at the first position where its distance is smaller than the other
                    // element's distance, which enables us to always keep a sorted list of at most k nearest
                    // neighbors.
                    if (distance2 <= it->second) {
                        it = nearestNodesWithSquaredDistances.insert(it, std::make_pair(neighborId, distance2));
                        bInserted = true;
                        break;
                    }
                }

                if (!bInserted && nearestNodesWithSquaredDistances.size() < nMaxNeighbors) {
                    nearestNodesWithSquaredDistances.emplace_back(std::make_pair(neighborId, distance2));
                }

                // We keep only the list of k nearest elements.
                if (bInserted && nearestNodesWithSquaredDistances.size() > nMaxNeighbors) {
                    nearestNodesWithSquaredDistances.pop_back();
                }
            }

            // Store nearest neighbor ids.
            int idx = 0;
            for (auto it = nearestNodesWithSquaredDistances.begin(); it != nearestNodesWithSquaredDistances.end(); ++it) {
                int neighborId = it->first;
                *graphEdges.mutable_data(nodeId, idx) = neighborId;
                idx++;
            }

            for (idx = nearestNodesWithSquaredDistances.size(); idx < nMaxNeighbors; idx++) {
                *graphEdges.mutable_data(nodeId, idx) = -1;
            }
        }

        return graphEdges;
    }

    inline int traverse_neighbors(const std::vector<std::set<int>>& node_neighbors, std::vector<int>& cluster_ids, int cluster_id, int node_id) {
        if (cluster_ids[node_id] != -1) return 0;

        std::set<int> active_node_indices;

        // Initialize with current node.
        int cluster_size = 0;
        active_node_indices.insert(node_id);

        // Process until we have no active nodes anymore.
        while (!active_node_indices.empty()) {
            int active_node_id = *active_node_indices.begin();
            active_node_indices.erase(active_node_indices.begin());

            if (cluster_ids[active_node_id] == -1) {
                cluster_ids[active_node_id] = cluster_id;
                ++cluster_size;
            }

            // Look if we need to process any of the neighbors
            for (const auto& n_idx : node_neighbors[active_node_id]) {
                if (cluster_ids[n_idx] == -1) {	// If it doesn't have a cluster yet
                    active_node_indices.insert(n_idx);
                }
            }
        }

        return cluster_size;
    }

    void node_and_edge_clean_up(const py::array_t<int>& graph_edges, py::array_t<bool>& valid_nodes_mask) {
        int num_nodes = graph_edges.shape(0);
        int max_num_neighbors = graph_edges.shape(1);

        std::list<int> removed_nodes;

        while (true) {
            int num_newly_removed_nodes = 0;

            for (int node_id = 0; node_id < num_nodes; ++node_id) {

                if (*valid_nodes_mask.data(node_id, 0) == false) {
                    // if node has been already removed, continue
                    continue;
                }

                int num_neighbors = 0;
                for (int i = 0; i < max_num_neighbors; ++i) {

                    int neighbor_id = *graph_edges.data(node_id, i);

                    // if neighboring node is -1, break, since by design 'graph_edges' has
                    // the shape [2, 3, 6, -1, -1, -1, -1, -1]
                    if (neighbor_id == -1) {
                        break;
                    }

                    // if neighboring node has been marked as invalid, continue
                    if (std::find(removed_nodes.begin(), removed_nodes.end(), neighbor_id) != removed_nodes.end()) {
                        continue;
                    }

                    ++num_neighbors;
                }

                if (num_neighbors <= 1) {
                    // remove node
                    *valid_nodes_mask.mutable_data(node_id, 0) = false;
                    removed_nodes.emplace_back(node_id);
                    // std::cout << "\tremoving node_id " << node_id << std::endl;
                    ++num_newly_removed_nodes;
                }
            }

            // std::cout << "num_newly_removed_nodes: " << num_newly_removed_nodes << std::endl;

            if (num_newly_removed_nodes == 0) {
                break;
            }
        }
    }

    std::vector<int> compute_clusters(
        const py::array_t<int> graph_edges,
        py::array_t<int> graph_clusters
    ) {
        int num_nodes = graph_edges.shape(0);
        int max_num_neighbors = graph_edges.shape(1);

        // convert graph_edges to a vector of sets
        std::vector<std::set<int>> node_neighbors(num_nodes);

        for (int node_id = 0; node_id < num_nodes; ++node_id) {
            for (int neighbor_idx = 0; neighbor_idx < max_num_neighbors; ++neighbor_idx) {

                int neighbor_id = *graph_edges.data(node_id, neighbor_idx);

                if (neighbor_id == -1) {
                    break;
                }

                node_neighbors[node_id].insert(neighbor_id);
                node_neighbors[neighbor_id].insert(node_id);
            }
        }

        std::vector<int> cluster_ids(num_nodes, -1);
        std::vector<int> clusters_size;

        int cluster_id = 0;
        for (int node_id = 0; node_id < num_nodes; ++node_id) {
            int cluster_size = traverse_neighbors(node_neighbors, cluster_ids, cluster_id, node_id);
            if (cluster_size > 0) {
                cluster_id++;
                clusters_size.push_back(cluster_size);
            }
        }

        for (int node_id = 0; node_id < num_nodes; ++node_id) {
            *graph_clusters.mutable_data(node_id, 0) = cluster_ids[node_id];
        }

        return clusters_size;
    }

    inline void compute_nearest_geodesic_nodes(
        const py::array_t<float>&  node_to_vertex_distance,
        const py::array_t<int>& valid_nodes_mask,
        const int vertex_id,
        std::vector<int>& nearest_geodesic_node_ids,
        std::vector<float>& dist_to_nearest_geodesic_nodes
    ) {
        int num_nodes = node_to_vertex_distance.shape(0);

        std::map<int, float> node_map;

        for (int n = 0; n < num_nodes; ++n) {

            // discard node if it was marked as invalid (due to not having enough neighbors)
            if (*valid_nodes_mask.data(n, 0) == false) {
                continue;
            }

            float dist = *node_to_vertex_distance.data(n, vertex_id);

            if (dist >= 0) {
                node_map.emplace(n, dist);
            }
        }

        // Sort the map by distance
        // Declaring the type of Predicate that accepts 2 pairs and return a bool
        typedef std::function<bool(std::pair<int, float>, std::pair<int, float>)> Comparator;

        // Defining a lambda function to compare two pairs. It will compare two pairs using second field
        Comparator comp_functor =
            [](std::pair<int, float> node1 ,std::pair<int, float> node2) {
                return node1.second < node2.second;
            };

        // Declaring a set that will store the pairs using above comparision logic
        std::set<std::pair<int, float>, Comparator> node_set(
            node_map.begin(), node_map.end(), comp_functor
        );

        for (auto n : node_set) {
            nearest_geodesic_node_ids.push_back(n.first);
            dist_to_nearest_geodesic_nodes.push_back(n.second);

            if (nearest_geodesic_node_ids.size() == GRAPH_K) {
                break;
            }
        }
    }

    void compute_pixel_anchors_geodesic(
        const py::array_t<float> &node_to_vertex_distance,
        const py::array_t<int> &valid_nodes_mask,
        const py::array_t<float> &vertices,
        const py::array_t<int> &vertex_pixels,
        py::array_t<int>& pixel_anchors,
        py::array_t<float>& pixel_weights,
        const int width, const int height,
        const float node_coverage
    ) {
        // Allocate graph node ids and corresponding skinning weights.
        // Initialize with invalid anchors.
        pixel_anchors.resize({ height, width, GRAPH_K }, false);
        pixel_weights.resize({ height, width, GRAPH_K }, false);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                for (int k = 0; k < GRAPH_K; k++) {
                    *pixel_anchors.mutable_data(y, x, k) = -1;
                    *pixel_weights.mutable_data(y, x, k) = 0.f;
                }
            }
        }

        int num_vertices = vertices.shape(0);

        for (int vertex_id = 0; vertex_id < num_vertices; vertex_id++) {
            // Get corresponding pixel location
            int u = *vertex_pixels.data(vertex_id, 0);
            int v = *vertex_pixels.data(vertex_id, 1);

            // Initialize some variables
            std::vector<int> nearest_geodesic_node_ids;
            std::vector<float> dist_to_nearest_geodesic_nodes;
            std::vector<float> skinning_weights;

            nearest_geodesic_node_ids.reserve(GRAPH_K);
            dist_to_nearest_geodesic_nodes.reserve(GRAPH_K);
            skinning_weights.reserve(GRAPH_K);

            // Find closest geodesic nodes
            compute_nearest_geodesic_nodes(
                node_to_vertex_distance, valid_nodes_mask, vertex_id,
                nearest_geodesic_node_ids, dist_to_nearest_geodesic_nodes
            );

            int num_anchors = nearest_geodesic_node_ids.size();

            // Compute skinning weights.
            float weight_sum{ 0.f };
            for (int i = 0; i < num_anchors; ++i) {
                float geodesic_dist_to_node = dist_to_nearest_geodesic_nodes[i];

                float weight = compute_anchor_weight(geodesic_dist_to_node, node_coverage);
                weight_sum += weight;

                skinning_weights.push_back(weight);
            }

            // Normalize the skinning weights.
            if (weight_sum > 0) {
                for (int i = 0; i < num_anchors; i++)
                    skinning_weights[i] /= weight_sum;
            }
            else if (num_anchors > 0) {
                for (int i = 0; i < num_anchors; i++)
                    skinning_weights[i] = 1.f / num_anchors;
            }

            // Store the results.
            for (int i = 0; i < num_anchors; i++) {
                *pixel_anchors.mutable_data(v, u, i) = nearest_geodesic_node_ids[i];
                *pixel_weights.mutable_data(v, u, i) = skinning_weights[i];
            }
        }
    }

    void compute_pixel_anchors_euclidean(
        const py::array_t<float>& graphNodes,
        const py::array_t<float>& pointImage,
        float nodeCoverage,
        py::array_t<int>& pixelAnchors,
        py::array_t<float>& pixelWeights
    ) {
        int nNodes = graphNodes.shape(0);
        int width = pointImage.shape(2);
        int height = pointImage.shape(1);
        // int nChannels = pointImage.shape(0);

        // Allocate graph node ids and corresponding skinning weights.
        // Initialize with invalid anchors.
        pixelAnchors.resize({ height, width, GRAPH_K }, false);
        pixelWeights.resize({ height, width, GRAPH_K }, false);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                for (int k = 0; k < GRAPH_K; k++) {
                    *pixelAnchors.mutable_data(y, x, k) = -1;
                    *pixelWeights.mutable_data(y, x, k) = 0.f;
                }
            }
        }

        // Compute anchors for every pixel.
        #pragma omp parallel for
        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                // Query 3d pixel position.
                Eigen::Vector3f pixelPos(*pointImage.data(0, y, x), *pointImage.data(1, y, x), *pointImage.data(2, y, x));
                if (pixelPos.z() <= 0) continue;

                // Keep only the k nearest Euclidean neighbors.
                std::list<std::pair<int, float>> nearestNodesWithSquaredDistances;

                for (int nodeId = 0; nodeId < nNodes; nodeId++) {
                    Eigen::Vector3f nodePos(*graphNodes.data(nodeId, 0), *graphNodes.data(nodeId, 1), *graphNodes.data(nodeId, 2));

                    float distance2 = (pixelPos - nodePos).squaredNorm();
                    bool bInserted = false;
                    for (auto it = nearestNodesWithSquaredDistances.begin(); it != nearestNodesWithSquaredDistances.end(); ++it) {
                        // We insert the element at the first position where its distance is smaller than the other
                        // element's distance, which enables us to always keep a sorted list of at most k nearest
                        // neighbors.
                        if (distance2 <= it->second) {
                            it = nearestNodesWithSquaredDistances.insert(it, std::make_pair(nodeId, distance2));
                            bInserted = true;
                            break;
                        }
                    }

                    if (!bInserted && nearestNodesWithSquaredDistances.size() < GRAPH_K) {
                        nearestNodesWithSquaredDistances.emplace_back(std::make_pair(nodeId, distance2));
                    }

                    // We keep only the list of k nearest elements.
                    if (bInserted && nearestNodesWithSquaredDistances.size() > GRAPH_K) {
                        nearestNodesWithSquaredDistances.pop_back();
                    }
                }

                // Compute skinning weights.
                std::vector<int> nearestEuclideanNodeIds;
                nearestEuclideanNodeIds.reserve(nearestNodesWithSquaredDistances.size());

                std::vector<float> skinningWeights;
                skinningWeights.reserve(nearestNodesWithSquaredDistances.size());

                float weightSum{ 0.f };
                for (auto it = nearestNodesWithSquaredDistances.begin(); it != nearestNodesWithSquaredDistances.end(); ++it) {
                    int nodeId = it->first;

                    Eigen::Vector3f nodePos(*graphNodes.data(nodeId, 0), *graphNodes.data(nodeId, 1), *graphNodes.data(nodeId, 2));
                    float weight = compute_anchor_weight(pixelPos, nodePos, nodeCoverage);
                    weightSum += weight;

                    nearestEuclideanNodeIds.push_back(nodeId);
                    skinningWeights.push_back(weight);
                }

                // Normalize the skinning weights.
                int nAnchors = nearestEuclideanNodeIds.size();

                if (weightSum > 0) {
                    for (int i = 0; i < nAnchors; i++)	skinningWeights[i] /= weightSum;
                }
                else if (nAnchors > 0) {
                    for (int i = 0; i < nAnchors; i++)	skinningWeights[i] = 1.f / nAnchors;
                }

                // Store the results.
                for (int i = 0; i < nAnchors; i++) {
                    *pixelAnchors.mutable_data(y, x, i) = nearestEuclideanNodeIds[i];
                    *pixelWeights.mutable_data(y, x, i) = skinningWeights[i];
                }
            }
        }
    }

    void construct_regular_graph(
        const py::array_t<float>& pointImage,
        int xNodes, int yNodes,
        float edgeThreshold,
        float maxPointToNodeDistance,
        float maxDepth,
        py::array_t<float>& graphNodes,
        py::array_t<int>& graphEdges,
        py::array_t<int>& pixelAnchors,
        py::array_t<float>& pixelWeights
    ) {
        int width = pointImage.shape(2);
        int height = pointImage.shape(1);
        int nChannels = pointImage.shape(0);

        float xStep = float(width - 1) / (xNodes - 1);
        float yStep = float(height - 1) / (yNodes - 1);

        // Sample graph nodes.
        // We need to maintain the mapping from all -> valid nodes ids.
        int nNodes = xNodes * yNodes;
        std::vector<int> sampledNodeMapping(nNodes, -1);

        std::vector<Eigen::Vector3f> nodePositions;
        nodePositions.reserve(nNodes);

        int nodeId = 0;
        for (int y = 0; y < yNodes; y++) {
            for (int x = 0; x < xNodes; x++) {
                int nodeIdx = y * xNodes + x;

                // We use nearest neighbor interpolation for node position
                // computation.
                int xPixel = std::round(x * xStep);
                int yPixel = std::round(y * yStep);

                Eigen::Vector3f pixelPos(*pointImage.data(0, yPixel, xPixel), *pointImage.data(1, yPixel, xPixel), *pointImage.data(2, yPixel, xPixel));
                if (pixelPos.z() <= 0 || pixelPos.z() > maxDepth) continue;

                nodePositions.push_back(pixelPos);
                sampledNodeMapping[nodeIdx] = nodeId;
                nodeId++;
            }
        }
        int nSampledNodes = nodeId;

        // Compute graph edges using pixel-wise connectivity. Each node
        // is connected with at most 8 neighboring pixels.
        int numNeighbors = 8;
        float edgeThreshold2 = edgeThreshold * edgeThreshold;

        std::vector<int> sampledNodeEdges(nSampledNodes * numNeighbors, -1);
        std::vector<bool> connectedNodes(nSampledNodes, false);

        int nConnectedNodes = 0;
        for (int y = 0; y < yNodes; y++) {
            for (int x = 0; x < xNodes; x++) {
                int nodeIdx = y * xNodes + x;
                int nodeId = sampledNodeMapping[nodeIdx];

                if (nodeId >= 0) {
                    Eigen::Vector3f nodePosition = nodePositions[nodeId];

                    int neighborCount = 0;
                    for (int yDelta = -1; yDelta <= 1; yDelta++) {
                        for (int xDelta = -1; xDelta <= 1; xDelta++) {
                            int xNeighbor = x + xDelta;
                            int yNeighbor = y + yDelta;
                            if (xNeighbor < 0 || xNeighbor >= xNodes || yNeighbor < 0 || yNeighbor >= yNodes)
                                continue;

                            int neighborIdx = yNeighbor * xNodes + xNeighbor;

                            if (neighborIdx == nodeIdx || neighborIdx < 0)
                                continue;

                            int neighborId = sampledNodeMapping[neighborIdx];
                            if (neighborId >= 0) {
                                Eigen::Vector3f neighborPosition = nodePositions[neighborId];

                                if ((neighborPosition - nodePosition).squaredNorm() <= edgeThreshold2) {
                                    sampledNodeEdges[nodeId * numNeighbors + neighborCount] = neighborId;
                                    neighborCount++;
                                }
                            }
                        }
                    }

                    for (int i = neighborCount; i < numNeighbors; i++) {
                        sampledNodeEdges[nodeId * numNeighbors + i] = -1;
                    }

                    if (neighborCount > 0) {
                        connectedNodes[nodeId] = true;
                        nConnectedNodes += 1;
                    }
                }
            }
        }

        // Filter out nodes with no edges.
        // After changing node ids the edge ids need to be changed as well.
        std::vector<int> validNodeMapping(nSampledNodes, -1);

        graphNodes.resize({ nConnectedNodes, 3 }, false);
        graphEdges.resize({ nConnectedNodes, numNeighbors }, false);

        int validNodeId = 0;
        for (int y = 0; y < yNodes; y++) {
            for (int x = 0; x < xNodes; x++) {
                int nodeIdx = y * xNodes + x;
                int nodeId = sampledNodeMapping[nodeIdx];

                if (nodeId >= 0 && connectedNodes[nodeId]) {
                    validNodeMapping[nodeId] = validNodeId;

                    Eigen::Vector3f nodePosition = nodePositions[nodeId];
                    *graphNodes.mutable_data(validNodeId, 0) = nodePosition.x();
                    *graphNodes.mutable_data(validNodeId, 1) = nodePosition.y();
                    *graphNodes.mutable_data(validNodeId, 2) = nodePosition.z();

                    validNodeId++;
                }
            }
        }

        for (int y = 0; y < yNodes; y++) {
            for (int x = 0; x < xNodes; x++) {
                int nodeIdx = y * xNodes + x;
                int nodeId = sampledNodeMapping[nodeIdx];

                if (nodeId >= 0 && connectedNodes[nodeId]) {
                    int validNodeId = validNodeMapping[nodeId];

                    if (validNodeId >= 0) {
                        for (int i = 0; i < numNeighbors; i++) {
                            int sampledNeighborId = sampledNodeEdges[nodeId * numNeighbors + i];
                            if (sampledNeighborId >= 0) {
                                *graphEdges.mutable_data(validNodeId, i) = validNodeMapping[sampledNeighborId];
                            }
                            else {
                                *graphEdges.mutable_data(validNodeId, i) = -1;
                            }
                        }
                    }
                }
            }
        }

        // Compute pixel anchors and weights.
        pixelAnchors.resize({ height, width, 4 }, false);
        pixelWeights.resize({ height, width, 4 }, false);

        float maxPointToNodeDistance2 = maxPointToNodeDistance * maxPointToNodeDistance;

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                // Initialize with invalid values.
                for (int k = 0; k < 4; k++) {
                    *pixelAnchors.mutable_data(y, x, k) = -1;
                    *pixelWeights.mutable_data(y, x, k) = 0.f;
                }

                // Compute 4 nearest nodes.
                float xNode = float(x) / xStep;
                float yNode = float(y) / yStep;

                int x0 = std::floor(xNode), x1 = x0 + 1;
                int y0 = std::floor(yNode), y1 = y0 + 1;

                // Check that all neighboring nodes are valid.
                if (x0 < 0 || x1 >= xNodes || y0 < 0 || y1 >= yNodes)
                    continue;

                int sampledNode00 = sampledNodeMapping[y0 * xNodes + x0];
                int sampledNode01 = sampledNodeMapping[y1 * xNodes + x0];
                int sampledNode10 = sampledNodeMapping[y0 * xNodes + x1];
                int sampledNode11 = sampledNodeMapping[y1 * xNodes + x1];

                if (sampledNode00 < 0 || sampledNode01 < 0 || sampledNode10 < 0 || sampledNode11 < 0)
                    continue;

                int validNode00 = validNodeMapping[sampledNode00];
                int validNode01 = validNodeMapping[sampledNode01];
                int validNode10 = validNodeMapping[sampledNode10];
                int validNode11 = validNodeMapping[sampledNode11];

                if (validNode00 < 0 || validNode01 < 0 || validNode10 < 0 || validNode11 < 0)
                    continue;

                // Check that all nodes are close enough to the point.
                Eigen::Vector3f pixelPos(*pointImage.data(0, y, x), *pointImage.data(1, y, x), *pointImage.data(2, y, x));
                if (pixelPos.z() <= 0 || pixelPos.z() > maxDepth) continue;

                if ((pixelPos - nodePositions[sampledNode00]).squaredNorm() > maxPointToNodeDistance2 ||
                    (pixelPos - nodePositions[sampledNode01]).squaredNorm() > maxPointToNodeDistance2 ||
                    (pixelPos - nodePositions[sampledNode10]).squaredNorm() > maxPointToNodeDistance2 ||
                    (pixelPos - nodePositions[sampledNode11]).squaredNorm() > maxPointToNodeDistance2
                ) {
                    continue;
                }

                // Compute bilinear weights.
                float dx = xNode - x0;
                float dy = yNode - y0;

                float w00 = (1 - dx) * (1 - dy);
                float w01 = (1 - dx) * dy;
                float w10 = dx * (1 - dy);
                float w11 = dx * dy;

                *pixelAnchors.mutable_data(y, x, 0) = validNode00;
                *pixelWeights.mutable_data(y, x, 0) = w00;
                *pixelAnchors.mutable_data(y, x, 1) = validNode01;
                *pixelWeights.mutable_data(y, x, 1) = w01;
                *pixelAnchors.mutable_data(y, x, 2) = validNode10;
                *pixelWeights.mutable_data(y, x, 2) = w10;
                *pixelAnchors.mutable_data(y, x, 3) = validNode11;
                *pixelWeights.mutable_data(y, x, 3) = w11;
            }
        }
    }

    void update_pixel_anchors(
        const std::map<int, int>& node_id_mapping,
        py::array_t<int>& pixel_anchors
    ) {
        int height      = pixel_anchors.shape(0);
        int width       = pixel_anchors.shape(1);
        int num_anchors = pixel_anchors.shape(2);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {

                for (int a = 0; a < num_anchors; a++) {

                    int current_anchor_id = *pixel_anchors.data(y, x, a);

                    if (current_anchor_id != -1) {
                        int mapped_anchor_id = node_id_mapping.at(current_anchor_id);

                        // update anchor only if it would actually change something
                        if (mapped_anchor_id != current_anchor_id) {
                            *pixel_anchors.mutable_data(y, x, a) = mapped_anchor_id;
                        }
                    }
                }
            }
        }
    }

} // namespace graph_proc


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2021, Oracle and/or its affiliates.
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_TEST_INTERSECTION_LINEAR_LINEAR_HPP
#define BOOST_GEOMETRY_TEST_INTERSECTION_LINEAR_LINEAR_HPP

#include <limits>

#include <boost/range/value_type.hpp>

#include <boost/geometry/geometry.hpp>
#include "../test_set_ops_linear_linear.hpp"
#include "../alternative_robustness_strategy.hpp"
#include <from_wkt.hpp>
#include <to_svg.hpp>

//! Contains (optional) settings such as tolerance
//! and to skip some test configurations
struct ut_settings
{
    ut_settings() = default;

    // Make it backwards compatible by a non-explicit constructor
    // such that just tolerance is also accepted
    ut_settings(const double t) : tolerance(t) {}

    double tolerance{std::numeric_limits<double>::epsilon()};

    // By default everything is tested.
    // Some rare cases might fail for some strategies, in that case
    // some strategies can be turned off
    bool test_invariance{true};
    bool test_reverse{true};

    bool test_default_strategy{true};
    bool test_explicit_strategy{true};
    bool test_alternative_side_strategy{true};
};

//==================================================================
//==================================================================
// intersection of (linear) geometries
//==================================================================
//==================================================================

template <typename Geometry1, typename Geometry2, typename MultiLineString>
inline void check_result(Geometry1 const& geometry1,
                         Geometry2 const& geometry2,
                         MultiLineString const& mls_output,
                         MultiLineString const& mls_int1,
                         MultiLineString const& mls_int2,
                         std::string const& case_id,
                         double tolerance)
{
    BOOST_CHECK_MESSAGE( equals::apply(mls_int1, mls_output, tolerance)
                         || equals::apply(mls_int2, mls_output, tolerance),
                         "case id: " << case_id
                         << " Expected: len=" << bg::length(mls_int1) << " count=" << bg::num_points(mls_int1)
                         << " or: len=" << bg::length(mls_int2) << " count=" << bg::num_points(mls_int2)
                         << " Detected: len=" << bg::length(mls_output) << " count=" << bg::num_points(mls_output)
                         << " wkt=" << std::setprecision(12) << bg::wkt(mls_output)
                         );
}

template
<
    typename Geometry1, typename Geometry2,
    typename MultiLineString
>
class test_intersection_of_geometries
{
private:
    static inline void base_test(Geometry1 const& geometry1,
                                 Geometry2 const& geometry2,
                                 MultiLineString const& mls_int1,
                                 MultiLineString const& mls_int2,
                                 std::string const& case_id,
                                 ut_settings const& settings,
                                 bool test_vector_and_deque = false)
    {
        static bool vector_deque_already_tested = false;

        typedef typename boost::range_value<MultiLineString>::type LineString;

        if (settings.test_default_strategy)
        {
            MultiLineString mls_output;
            bg::intersection(geometry1, geometry2, mls_output);

            check_result(geometry1, geometry2, mls_output, mls_int1, mls_int2,
                         case_id, settings.tolerance);

#ifdef TEST_WITH_SVG
            to_svg(geometry1, geometry2, mls_output, case_id);
            set_operation_output("intersection", case_id,
                                 geometry1, geometry2, mls_output);
#endif
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "Geometry #1: " << bg::wkt(geometry1) << std::endl;
            std::cout << "Geometry #2: " << bg::wkt(geometry2) << std::endl;
            std::cout << "intersection : " << bg::wkt(mls_output) << std::endl;
            std::cout << "expected intersection : " << bg::wkt(mls_int1)
                      << " or: " << bg::wkt(mls_int2) << std::endl;
            std::cout << std::endl;
            std::cout << "************************************" << std::endl;
            std::cout << std::endl;
            std::cout << std::endl;
#endif
        }

        if (settings.test_explicit_strategy)
        {
            MultiLineString mls_output;
            using strategy_type
                = typename bg::strategy::relate::services::default_strategy
                <
                    Geometry1, Geometry2
                >::type ;
            bg::intersection(geometry1, geometry2, mls_output, strategy_type());

#ifdef TEST_WITH_SVG
            to_svg(geometry1, geometry2, mls_output, case_id + "_explicit");
#endif

            check_result(geometry1, geometry2, mls_output, mls_int1, mls_int2,
                         case_id, settings.tolerance);
        }

        if (settings.test_alternative_side_strategy)
        {
            using side_strategy_type = bg::strategy::side::side_robust<>;

            MultiLineString mls_output;
            bg::intersection(geometry1, geometry2, mls_output,
                             alternative_robustness_strategy<side_strategy_type>());

#ifdef TEST_WITH_SVG
            to_svg(geometry1, geometry2, mls_output, case_id + "_alternative");
#endif

            check_result(geometry1, geometry2, mls_output, mls_int1, mls_int2,
                         case_id, settings.tolerance);
        }

        if (! vector_deque_already_tested && test_vector_and_deque)
        {
            vector_deque_already_tested = true;

            typedef std::vector<LineString> linestring_vector;
            typedef std::deque<LineString> linestring_deque;
            linestring_vector ls_vector_output;
            linestring_deque ls_deque_output;

#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << std::endl;
            std::cout << "Testing with vector and deque as output container..."
                      << std::endl;
#endif
            bg::intersection(geometry1, geometry2, ls_vector_output);
            bg::intersection(geometry1, geometry2, ls_deque_output);

            BOOST_CHECK(multilinestring_equals
                        <
                            false
                        >::apply(mls_int1, ls_vector_output, settings.tolerance));

            BOOST_CHECK(multilinestring_equals
                        <
                            false
                        >::apply(mls_int1, ls_deque_output, settings.tolerance));

#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "Done!" << std::endl << std::endl;
#endif
        }

        if (settings.test_reverse)
        {
            // check the intersection where the order of the two
            // geometries is reversed
            MultiLineString mls_output;
            bg::intersection(geometry2, geometry1, mls_output);

            check_result(geometry1, geometry2, mls_output, mls_int1, mls_int2,
                         case_id, settings.tolerance);
        }
    }

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    static inline void base_test_all(Geometry1 const& geometry1,
                                     Geometry2 const& geometry2)
    {
        typedef typename bg::point_type<MultiLineString>::type Point;
        typedef bg::model::multi_point<Point> multi_point;

        MultiLineString mls12_output, mls21_output;
        multi_point mp12_output, mp21_output;

        bg::intersection(geometry1, geometry2, mls12_output);
        bg::intersection(geometry1, geometry2, mp12_output);
        bg::intersection(geometry2, geometry1, mls21_output);
        bg::intersection(geometry2, geometry1, mp21_output);

        std::cout << "************************************" << std::endl;
        std::cout << "Geometry #1: " << bg::wkt(geometry1) << std::endl;
        std::cout << "Geometry #2: " << bg::wkt(geometry2) << std::endl;
        std::cout << "intersection(1,2) [MLS]: " << bg::wkt(mls12_output)
                  << std::endl;
        std::cout << "intersection(2,1) [MLS]: " << bg::wkt(mls21_output)
                  << std::endl;
        std::cout << std::endl;
        std::cout << "intersection(1,2) [MP]: " << bg::wkt(mp12_output)
                  << std::endl;
        std::cout << "intersection(2,1) [MP]: " << bg::wkt(mp21_output)
                  << std::endl;
        std::cout << std::endl;
        std::cout << "************************************" << std::endl;
        std::cout << std::endl;
        std::cout << std::endl;
    }
#else
    static inline void base_test_all(Geometry1 const&, Geometry2 const&)
    {
    }
#endif

public:
    static inline void apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             MultiLineString const& mls_int1,
                             MultiLineString const& mls_int2,
                             std::string const& case_id,
                             ut_settings const& settings = ut_settings())
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "test case: " << case_id << std::endl;
#endif
        BOOST_CHECK(settings.test_default_strategy
                    || settings.test_explicit_strategy
                    || settings.test_alternative_side_strategy);

        Geometry1 rg1(geometry1);
        bg::reverse<Geometry1>(rg1);

        Geometry2 rg2(geometry2);
        bg::reverse<Geometry2>(rg2);

        if (settings.test_invariance)
        {
            static const bool linear = bg::util::is_linear<Geometry1>::value
                                    && bg::util::is_linear<Geometry2>::value;

            test_get_turns_ll_invariance<linear>::apply(geometry1, geometry2);
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << std::endl
                      << "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
                      << std::endl << std::endl;
#endif
            test_get_turns_ll_invariance<linear>::apply(rg1, geometry2);
        }

        base_test(geometry1, geometry2, mls_int1, mls_int2, case_id, settings);
        base_test_all(geometry1, geometry2);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << std::endl;
        std::cout << std::endl;
#endif
    }

    static inline void apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             MultiLineString const& mls_int,
                             std::string const& case_id,
                             ut_settings const& settings = ut_settings())
    {
        apply(geometry1, geometry2, mls_int, mls_int, case_id, settings);
    }
};

#endif // BOOST_GEOMETRY_TEST_INTERSECTION_LINEAR_LINEAR_HPP


// Boost.Geometry
// Unit Test

// Copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <string>

#include "test_union.hpp"

#include <boost/geometry/geometries/point_xy.hpp>

struct exterior_points_counter
{
    exterior_points_counter() : count(0) {}

    template <typename Polygon>
    void operator()(Polygon const& poly)
    {
        count += boost::size(bg::exterior_ring(poly));
    }

    std::size_t count;
};

struct interiors_counter
    : exterior_points_counter
{
    template <typename Polygon>
    void operator()(Polygon const& poly)
    {
        count += boost::size(bg::interior_rings(poly));
    }
};

void test_geographic_one(std::string const& wkt1, std::string const& wkt2,
                         std::size_t count, std::size_t exterior_points_count, std::size_t interiors_count,
                         double expected_area)
{
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point;
    typedef bg::model::polygon<point> polygon;
    typedef bg::model::multi_polygon<polygon> multipolygon;

    bg::srs::spheroid<double> sph(6378137.0000000000, 6356752.3142451793);

    bg::strategy::intersection::geographic_segments<> is(sph);
    bg::strategy::area::geographic<point> as(sph);

    polygon p1, p2;

    boost::geometry::read_wkt(wkt1, p1);
    boost::geometry::read_wkt(wkt2, p2);

    multipolygon result;

    enum test_mode { expect_valid, expect_empty, expect_exception };

#if defined(BOOST_GEOMETRY_UNION_THROW_INVALID_OUTPUT_EXCEPTION)
    test_mode mode = expected_area >= 0 ? expect_valid : expect_exception;
#elif defined(BOOST_GEOMETRY_UNION_RETURN_INVALID)
    test_mode mode = expect_valid;
#else
    // default
    test_mode mode = expected_area >= 0 ? expect_valid : expect_empty;
#endif

    if (mode == expect_exception)
    {
        BOOST_CHECK_THROW(boost::geometry::union_(p1, p2, result, is),
                          bg::invalid_output_exception);
    }
    else
    {

        boost::geometry::union_(p1, p2, result, is);

        double result_area = bg::area(result, as);

        std::size_t result_count = boost::size(result);
        std::size_t result_exterior_points = std::for_each(boost::begin(result),
                                                           boost::end(result),
                                                           exterior_points_counter()).count;
        std::size_t result_interiors = std::for_each(boost::begin(result),
                                                     boost::end(result),
                                                     interiors_counter()).count;
        if (mode == expect_valid)
        {
            BOOST_CHECK_EQUAL(result_count, count);
            BOOST_CHECK_EQUAL(result_exterior_points, exterior_points_count);
            BOOST_CHECK_EQUAL(result_interiors, interiors_count);
            BOOST_CHECK_CLOSE(result_area, expected_area, 0.001);
        }
        else
        {
            BOOST_CHECK_EQUAL(result_count, 0);
            BOOST_CHECK_EQUAL(result_area, 0);
        }
    }
}

void test_geographic()
{
    // input ok and the result is ok
    test_geographic_one("POLYGON((16 15,-132 10,-56 89,67 5,16 15))",
                        "POLYGON((101 49,12 40,-164 10,117 0,101 49))",
                        1, 9, 0, 144265751613509.06);

    // input ok but the result is too big
    test_geographic_one("POLYGON((16 -15,-132 -22,-56 89,67 -29,16 -15))",
                        "POLYGON((101 49,12 40,-164 -21,117 -61,101 49))",
                        1, 9, 0, -163427005620080.0);

    // the second polygon is reversed i.e. it covers more than half of the globe
    // so the result is also too big
    test_geographic_one("POLYGON((16 -15,-132 -22,-56 89,67 -29,16 -15))",
                        "POLYGON((101 49,117 -61,-164 -21,12 40,101 49))",
                        1, 7, 0, -125258931656228.08);
}

int test_main(int, char* [])
{
    test_geographic();

    return 0;
}


#ifndef HALFEDGE_CELLCOMPLEX_H
#define HALFEDGE_CELLCOMPLEX_H
#include <Eigen/Geometry>
#include <array>
#include <iostream>
#include <map>
#include <set>
#include <tuple>
#include <utility>
#include <vector>

#include "mtao/data_structures/disjoint_set.hpp"
#include "mtao/geometry/cyclic_order.hpp"
#include "mtao/geometry/mesh/earclipping.hpp"
#include "mtao/types.hpp"

namespace mtao::geometry::mesh {

struct cell_iterator;
struct vertex_iterator;
struct boundary_iterator;

class HalfEdgeMesh {
   public:
    enum class Index {
        VertexIndex = 0,
        CellIndex = 1,
        NextIndex = 2,
        DualIndex = 3,
        IndexEnd = 4
    };
    using VecXi = mtao::VectorX<int>;
    template <int D>
    using ColVectorsi = ColVectors<int, D>;
    using Cells = mtao::MatrixX<int>;
    using Edges = ColVectorsi<int(Index::IndexEnd)>;
    using DatVec = mtao::Vector<int, int(Index::IndexEnd)>;

    struct HalfEdge;

    HalfEdgeMesh(const std::string& str);
    HalfEdgeMesh(const Edges& E);
    static HalfEdgeMesh from_cells(const Cells& F,
                                   bool use_open_halfedges = false);
    static HalfEdgeMesh from_edges(
        const ColVectors<int, 2>& E);  // From non-dual edges
    HalfEdgeMesh() = default;
    HalfEdgeMesh(const HalfEdgeMesh&) = default;
    HalfEdgeMesh(HalfEdgeMesh&&) = default;
    HalfEdgeMesh& operator=(const HalfEdgeMesh&) = default;
    HalfEdgeMesh& operator=(HalfEdgeMesh&&) = default;

    auto vertex_indices() { return m_edges.row(int(Index::VertexIndex)); }
    auto cell_indices() { return m_edges.row(int(Index::CellIndex)); }
    auto next_indices() { return m_edges.row(int(Index::NextIndex)); }
    auto dual_indices() { return m_edges.row(int(Index::DualIndex)); }
    auto vertex_indices() const { return m_edges.row(int(Index::VertexIndex)); }
    auto cell_indices() const { return m_edges.row(int(Index::CellIndex)); }
    auto next_indices() const { return m_edges.row(int(Index::NextIndex)); }
    auto dual_indices() const { return m_edges.row(int(Index::DualIndex)); }

    int vertex_index(int idx) const {
        if (idx < 0 || idx >= size())
            return -1;
        else
            return vertex_indices()(idx);
    }
    int dual_index(int idx) const {
        if (idx < 0 || idx >= size())
            return -1;
        else
            return dual_indices()(idx);
    }
    int next_index(int idx) const {
        if (idx < 0 || idx >= size())
            return -1;
        else
            return next_indices()(idx);
    }
    int cell_index(int idx) const {
        if (idx < 0 || idx >= size())
            return -1;
        else
            return cell_indices()(idx);
    }
    const Edges& edges() const { return m_edges; }
    template <typename TDerived = ColVectors<int, 2>>
    void make_topology(const std::map<std::array<int, 2>, std::tuple<int, bool>>&
                           tangent_map,
                       const Eigen::MatrixBase<TDerived>& T);

    // Returns a unique halfedge to access a particular element type
    std::vector<int> cell_halfedges() const;
    std::vector<int> vertex_halfedges() const;
    std::vector<int> boundary_halfedges() const;
    std::map<int, int> cell_halfedges_map() const;
    std::map<int, int> vertex_halfedges_map() const;
    std::map<int, std::set<int>> cell_collect_all_halfedges() const;

    std::vector<std::set<int>> cell_halfedges_multi_component() const;
    std::map<int, std::set<int>> cell_halfedges_multi_component_map() const;

    cell_iterator get_cell_iterator(const HalfEdge& he) const;
    vertex_iterator get_vertex_iterator(const HalfEdge& he) const;
    boundary_iterator get_boundary_iterator(const HalfEdge& he) const;
    cell_iterator get_cell_iterator(int he_idx) const;
    vertex_iterator get_vertex_iterator(int he_idx) const;
    boundary_iterator get_boundary_iterator(int he_idx) const;

    std::vector<std::vector<int>> cells() const;
    std::map<int, std::vector<int>> cells_map() const;
    std::map<int, std::set<std::vector<int>>> cells_multi_component_map() const;
    std::map<int, std::set<int>> halfedges_per_cell() const;
    std::vector<int> cell(int cell_index) const;
    std::vector<int> cell(const HalfEdge& e) const;
    std::vector<int> cell_he(int he_in_cell) const;

    std::map<int, std::set<int>> vertices_per_cell() const;

    std::vector<std::vector<int>> dual_cells() const;
    std::map<int, std::vector<int>> dual_cells_map() const;
    std::vector<int> dual_cell(int vertex_index) const;
    std::vector<int> dual_cell(const HalfEdge& e) const;
    std::vector<int> dual_cell_he(int he_in_cell) const;

    std::vector<int> one_ring(int vertex_idx) const;
    std::vector<int> one_ring(const HalfEdge& e) const;
    std::vector<int> one_ring_he(int he_pointing_to_vertex) const;

    HalfEdge edge(int i) const;
    HalfEdge cell_edge(int i) const;
    HalfEdge vertex_edge(int i) const;
    std::set<HalfEdge> cell_edges(int i) const;
    std::set<HalfEdge> vertex_edges(int i) const;

    // Edge {i,j} => halfedge pointing to j, where i is the dual
    std::map<std::array<int, 2>, int> edge_to_halfedge(
        bool use_next = false) const;

    int size() const { return m_edges.cols(); }
    int boundary_size() const;
    int num_cells() const;
    int num_vertices() const;

    bool is_boundary(int index) const;
    bool is_boundary_vertex(int index) const;
    bool is_boundary_cell(int index) const;

    HalfEdgeMesh submesh_from_cells(const std::set<int>& cell_indices) const;
    HalfEdgeMesh submesh_from_edges(const std::set<int>& edge_indices) const;
    HalfEdgeMesh submesh_from_vertices(const std::set<int>& edge_indices) const;

    // returns the directed halfedges that point away from each particular
    // vertex
    std::map<int, std::set<int>> vertex_edges_no_topology() const;

    void set_one_ring_adjacencies_quadcross(
        const std::vector<int>& ordered_edge_indices);
    template <typename TDerived = ColVectors<int, 2>>
        // get passed in half-edge indices
    void set_one_ring_adjacencies_quadcross(
        const std::set<int>& edges,
        const std::map<std::array<int, 2>, std::tuple<int, bool>>& tangent_map,
        const Eigen::MatrixBase<TDerived>& T);

    template <typename Derived, typename Derived2>
    typename Derived::Scalar winding_number(
        const Eigen::MatrixBase<Derived>& V, const HalfEdge& cell_edge,
        const Eigen::MatrixBase<Derived2>& p) const;
    template <typename Derived, typename Derived2>
    typename Derived::Scalar winding_number(
        const Eigen::MatrixBase<Derived>& V, const std::set<int>& cell_edge,
        const Eigen::MatrixBase<Derived2>& p) const;
    template <typename Derived>
    typename Derived::Scalar signed_area(const Eigen::MatrixBase<Derived>& V,
                                         const HalfEdge& cell_edge) const;
    template <typename Derived>
    typename Derived::Scalar signed_area(const Eigen::MatrixBase<Derived>& V,
                                         const std::set<int>& cell_edge) const;
    template <typename Derived>
    std::map<int, typename Derived::Scalar> signed_areas(
        const Eigen::MatrixBase<Derived>& V) const;
    template <typename Derived>
    typename Derived::Scalar area(const Eigen::MatrixBase<Derived>& V,
                                  const HalfEdge& cell_edge) const {
        return std::abs(signed_area(V, cell_edge));
    }
    template <typename Derived>
    typename Derived::Scalar area(const Eigen::MatrixBase<Derived>& V,
                                  const std::set<int>& cell_edge) const {
        return std::abs(signed_area(V, cell_edge));
    }

    template <typename Derived, typename Derived2>
    bool is_inside(const Eigen::MatrixBase<Derived>& V,
                   const std::set<int>& cell_edge,
                   const Eigen::MatrixBase<Derived2>& p) const;
    template <typename Derived>
    bool is_inside(const Eigen::MatrixBase<Derived>& V,
                   const std::set<int>& cell_edge,
                   const std::vector<int>& vertex_indices) const;

    template <typename Derived, typename Derived2>
    bool is_inside(const Eigen::MatrixBase<Derived>& V, int cell_edge,
                   const Eigen::MatrixBase<Derived2>& p) const;
    template <typename Derived, typename Derived2>
    bool is_inside(const Eigen::MatrixBase<Derived>& V,
                   const HalfEdge& cell_edge,
                   const Eigen::MatrixBase<Derived2>& p) const;
    template <typename Derived>
    bool is_inside(const Eigen::MatrixBase<Derived>& V, int cell_edge,
                   const std::vector<int>& vertex_indices) const;
    template <typename Derived>
    bool is_inside(const Eigen::MatrixBase<Derived>& V,
                   const HalfEdge& cell_edge,
                   const std::vector<int>& vertex_indices) const;

    template <typename Derived>
    void tie_nonsimple_cells(const Eigen::MatrixBase<Derived>& V,
                             const std::set<int>& cell_halfedges);
    template <typename Derived>
    void tie_nonsimple_cells(const Eigen::MatrixBase<Derived>& V) {
        std::vector<int> C = cell_halfedges();
        tie_nonsimple_cells(V, std::set<int>(C.begin(), C.end()));
    }

    void make_cells();  // assumes duals and cells nexts are set up, makes every
                        // element part of some cell

    bool is_island(int cell_idx) const;
    bool is_island(const HalfEdge& e) const;
    bool is_island_he(int he_in_cell) const;

    bool check_cell_validity() const;
    bool check_vertex_validity() const;
    bool check_halfedge_reachability() const;

   private:
    void construct(const Cells& F);
    void construct_open_halfedges(const Cells& F);
    void clear(size_t new_size = 0);
    void complete_boundary_cells();

   private:
    Edges m_edges;
};

struct HalfEdgeMesh::HalfEdge {
   public:
    using MeshType = HalfEdgeMesh;
    HalfEdge(const HalfEdge&) = default;
    HalfEdge(HalfEdge&&) = default;
    HalfEdge(const MeshType* cc, int idx = -1);
    int index() const { return m_index; }
    int vertex() const { return m_cc->vertex_index(index()); }
    int cell() const { return m_cc->cell_index(index()); }
    int next_index() const;
    int dual_index() const;
    HalfEdge get_next() const;
    HalfEdge get_dual() const;
    HalfEdge& next();
    HalfEdge& dual();

    explicit operator int() const { return m_index; }
    explicit operator bool() const { return m_index != -1; }
    bool operator<(const HalfEdge& other) const {
        return m_index < other.m_index;
    }

   private:
    const MeshType* m_cc;
    int m_index = -1;
};
template <typename S, int D>
class EmbeddedHalfEdgeMesh : public HalfEdgeMesh {
   public:
    using Vec = mtao::Vector<S, D>;

    EmbeddedHalfEdgeMesh() = default;
    EmbeddedHalfEdgeMesh(const EmbeddedHalfEdgeMesh&) = default;
    EmbeddedHalfEdgeMesh(EmbeddedHalfEdgeMesh&&) = default;
    EmbeddedHalfEdgeMesh& operator=(const EmbeddedHalfEdgeMesh&) = default;
    EmbeddedHalfEdgeMesh& operator=(EmbeddedHalfEdgeMesh&&) = default;
    ~EmbeddedHalfEdgeMesh() = default;
    template <typename... Args>
    EmbeddedHalfEdgeMesh(const ColVectors<S, D>& V, Args&&... args)
        : HalfEdgeMesh(std::forward<Args>(args)...), m_vertices(V) {}

    static EmbeddedHalfEdgeMesh from_cells(const ColVectors<S, D>& V,
                                           const Cells& F,
                                           bool use_open_halfedges = false) {
        return EmbeddedHalfEdgeMesh(
            V, HalfEdgeMesh::from_cells(F, use_open_halfedges));
    }
    static EmbeddedHalfEdgeMesh from_edges(const ColVectors<S, D>& V,
                                           const ColVectors<int, 2>& E) {
        return EmbeddedHalfEdgeMesh(V, HalfEdgeMesh::from_edges(E));
    }

    ColVectors<int, 3> cells_triangulated() const;
    auto V(int i) {
        assert(i >= 0);
        assert(i < nV());
        return m_vertices.col(i);
    }
    auto V(int i) const {
        assert(i >= 0);
        assert(i < nV());
        return m_vertices.col(i);
    }
    auto&& V() const { return m_vertices; }
    int nV() const { return m_vertices.cols(); }

    void make_topology();
    void tie_nonsimple_cells(const std::set<int>& cell_halfedges) const {
        return HalfEdgeMesh::tie_nonsimple_cells(m_vertices, cell_halfedges);
    }

    void tie_nonsimple_cells() {
        std::vector<int> C = cell_halfedges();
        return HalfEdgeMesh::tie_nonsimple_cells(
            m_vertices, std::set<int>(C.begin(), C.end()));
    }

    template <typename Derived>
    void set_one_ring_adjacencies_quadcross(
        const Eigen::MatrixBase<Derived>& central_vertex,
        const std::set<int>& outward_facing_halfedges);

    template <typename Derived>
    bool is_inside(const std::set<int>& cell_edge,
                   const Eigen::MatrixBase<Derived>& p) const;
    bool is_inside(const std::set<int>& cell_edge,
                   const std::vector<int>& vertex_indices) const;

    template <typename Derived>
    bool is_inside(int cell_edge, const Eigen::MatrixBase<Derived>& p) const;
    bool is_inside(int cell_edge, const std::vector<int>& vertex_indices) const;

    template <typename Derived>
    bool is_inside(const HalfEdge& cell_edge,
                   const Eigen::MatrixBase<Derived>& p) const;
    bool is_inside(const HalfEdge& cell_edge,
                   const std::vector<int>& vertex_indices) const;
    template <typename Derived>
    int get_cell(const Eigen::MatrixBase<Derived>& p)
        const;  // returns the cell containing point p
    mtao::VectorX<int> get_cells(const ColVectors<S, D>& P) const;
    auto T(int idx) const {
        auto e = edge(idx);
        int a = e.vertex();
        int b = e.dual().vertex();
        return V(b) - V(a);
    }

    template <typename Derived>
    S winding_number(const std::set<int>& cell_edge,
                     const Eigen::MatrixBase<Derived>& p) const {
        return HalfEdgeMesh::winding_number(m_vertices, cell_edge, p);
    }
    S signed_area(const std::set<int>& cell_edge) const {
        return HalfEdgeMesh::signed_area(m_vertices, cell_edge);
    }
    S area(const std::set<int>& cell_edge) const {
        return HalfEdgeMesh::area(m_vertices, cell_edge);
    }
    template <typename Derived>
    S winding_number(const HalfEdge& cell_edge,
                     const Eigen::MatrixBase<Derived>& p) const {
        return HalfEdgeMesh::winding_number(m_vertices, cell_edge, p);
    }
    S signed_area(const HalfEdge& cell_edge) const {
        return HalfEdgeMesh::signed_area(m_vertices, cell_edge);
    }
    S area(const HalfEdge& cell_edge) const {
        return HalfEdgeMesh::area(m_vertices, cell_edge);
    }

    std::map<int, S> signed_areas() const {
        return HalfEdgeMesh::signed_areas(m_vertices);
    }

    std::set<std::tuple<S, int>> get_edge_angles(int eidx) const;

    std::map<int, std::set<int>> cell_containment() const;

   private:
    // creates next arrows
    ColVectors<S, D> m_vertices;
};
namespace detail {
void invalid_edge_warning();
void invalid_cell_warning();
}  // namespace detail
}  // namespace mtao::geometry::mesh

#include "mtao/geometry/mesh/halfedge_impl.hpp"
#include "mtao/geometry/mesh/halfedge_iterator.hpp"

#endif  // HALFEDGE_CELLCOMPLEX_H


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <NTL/ZZ.h>
#include <cassert>
#include <helib/norms.h>
#include <helib/EncryptedArray.h>
#include <helib/ArgMap.h>

NTL_CLIENT
using namespace helib;

#ifdef DEBUG_PRINTOUT
#include <helib/debugging.h>
#endif

bool noPrint = true;

int main(int argc, char *argv[])
{
  ArgMap amap;
  amap.arg("noPrint", noPrint, "suppress printouts");

  long m=16;
  amap.arg("m", m, "cyclotomic index");
  amap.note("e.g., m=1024, m=2047");

  long r=8;
  amap.arg("r", r, "bit of precision");
  amap.parse(argc, argv);

  if (!noPrint) {
    vector<long> f;
    factorize(f,m);
    cout << "r="<<r<<", factoring "<<m<<" gives [";
    for (unsigned long i=0; i<f.size(); i++)
      cout << f[i] << " ";
    cout << "]\n";
  }

  Context context(m, /*p=*/-1, r);
  buildModChain(context, 5, 2);

  const EncryptedArrayCx& ea = context.ea->getCx();
  if (!noPrint)
    ea.getPAlgebra().printout();

#ifdef DEBUG_PRINTOUT
  vector<cx_double> vc1;
  ea.random(vc1);
  cout << "random complex vc1=";
  printVec(cout,vc1,8)<<endl;

  vector<double> vd;
  ea.random(vd);
  cout << "random real vd=";
  printVec(cout,vd,8)<<endl;
#endif

  vector<double> vl;
  ea.random(vl);
  vl[1] = -1; // ensure that this is not the zero vector
#ifdef DEBUG_PRINTOUT
  cout << "random int v=";
  printVec(cout,vl,8)<<endl;
#endif

  zzX poly;
  double factor = ea.encode(poly, vl, 1.0);
  if (!noPrint) {
    ZZX poly2;
    convert(poly2, poly);
    cout << "  encoded into a degree-"<<NTL::deg(poly2)<<" polynomial\n";
  }

  vector<double> vd2;
  ea.decode(vd2, poly, factor);
#ifdef DEBUG_PRINTOUT
  cout << "  decoded into vd2=";
  printVec(cout,vd2,8)<<endl;
#endif
  assert(lsize(vl)==lsize(vd2));

  double maxDiff = 0.0;
  for (long i=0; i<lsize(vl); i++) {
    double diffAbs = std::abs(vl[i]-vd2[i]);
    if (diffAbs > maxDiff)
      maxDiff = diffAbs;
  }
  cout << ((maxDiff>0.1)? "BAD?" : "GOOD?")
       << "  max |v-vd2|_{infty}="<<maxDiff
       << endl;
  return 0;
}


#pragma once

#include <crab/cfg/cfg.hpp>
#include <crab/cfg/cfg_bgl.hpp> // needed by wto.hpp
#include <crab/common/debug.hpp>
#include <crab/domains/interval.hpp>
#include <crab/domains/linear_constraints.hpp>
#include <crab/iterators/wto.hpp>

#include <boost/range/iterator_range.hpp>

#include <algorithm>
#include <climits>
#include <unordered_map>

namespace crab {

namespace iterators {

/**
    Class that represents a set of thresholds used by the widening operator
**/
template <typename Number> class thresholds {

private:
  /// XXX: internal representation of a threshold
  typedef ikos::bound<Number> bound_t;

  std::vector<bound_t> m_thresholds;
  unsigned int m_size;

  template <class B1, class B2> static B2 convert_bounds_impl(B1 b1) {
    B2 b2(0); // some initial value it doesn't matter which one
    ikos::bounds_impl::convert_bounds(b1, b2);
    return b2;
  }

public:
  thresholds(int size = UINT_MAX) : m_size(size) {
    m_thresholds.push_back(bound_t::minus_infinity());
    m_thresholds.push_back(0);
// useful thresholds for wrapped domains
#if 0
	 m_thresholds.push_back(bound_t("-2147483648"));
	 m_thresholds.push_back(bound_t("-32768"));
	 m_thresholds.push_back(bound_t("-128"));
	 m_thresholds.push_back(bound_t("127"));
	 m_thresholds.push_back(bound_t("255"));
	 m_thresholds.push_back(bound_t("32767"));
	 m_thresholds.push_back(bound_t("65535"));
	 m_thresholds.push_back(bound_t("2147483647"));
#endif
    m_thresholds.push_back(bound_t::plus_infinity());
  }

  unsigned size(void) const { return m_thresholds.size(); }

  template <typename N> void add(ikos::bound<N> v1) {
    if (m_thresholds.size() < m_size) {
      bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t>(v1);
      if (std::find(m_thresholds.begin(), m_thresholds.end(), v) ==
          m_thresholds.end()) {
        auto ub = std::upper_bound(m_thresholds.begin(), m_thresholds.end(), v);

        // don't add consecutive thresholds
        if (v > 0) {
          auto prev = ub;
          --prev;
          if (prev != m_thresholds.begin()) {
            if (*prev + 1 == v) {
              *prev = v;
              return;
            }
          }
        } else if (v < 0) {
          if (*ub - 1 == v) {
            *ub = v;
            return;
          }
        }

        m_thresholds.insert(ub, v);
      }
    }
  }

  template <typename N> ikos::bound<N> get_next(ikos::bound<N> v1) const {
    if (v1.is_plus_infinity())
      return v1;
    bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t>(v1);
    bound_t t = m_thresholds[m_thresholds.size() - 1];
    auto ub = std::upper_bound(m_thresholds.begin(), m_thresholds.end(), v);
    if (ub != m_thresholds.end())
      t = *ub;
    return convert_bounds_impl<bound_t, ikos::bound<N>>(t);
  }

  template <typename N> ikos::bound<N> get_prev(ikos::bound<N> v1) const {
    if (v1.is_minus_infinity())
      return v1;
    bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t>(v1);
    auto lb = std::lower_bound(m_thresholds.begin(), m_thresholds.end(), v);
    if (lb != m_thresholds.end()) {
      --lb;
      if (lb != m_thresholds.end()) {
        return convert_bounds_impl<bound_t, ikos::bound<N>>(*lb);
      }
    }
    return convert_bounds_impl<bound_t, ikos::bound<N>>(m_thresholds[0]);
  }

  void write(crab_os &o) const {
    o << "{";
    for (typename std::vector<bound_t>::const_iterator
             it = m_thresholds.begin(),
             et = m_thresholds.end();
         it != et;) {
      bound_t b(*it);
      b.write(o);
      ++it;
      if (it != m_thresholds.end())
        o << ",";
    }
    o << "}";
  }
};

template <typename Number>
inline crab_os &operator<<(crab_os &o, const thresholds<Number> &t) {
  t.write(o);
  return o;
}

/**
   Collect thresholds per wto cycle (i.e. loop)
**/
template <typename CFG>
class wto_thresholds : public ikos::wto_component_visitor<CFG> {

public:
  typedef typename CFG::basic_block_label_t basic_block_label_t;
  typedef ikos::wto_vertex<CFG> wto_vertex_t;
  typedef ikos::wto_cycle<CFG> wto_cycle_t;
  typedef crab::iterators::thresholds<typename CFG::number_t> thresholds_t;
  typedef std::unordered_map<basic_block_label_t, thresholds_t>
      thresholds_map_t;

private:
  // the cfg
  CFG m_cfg;
  // maximum number of thresholds
  size_t m_max_size;
  // keep a set of thresholds per wto head
  thresholds_map_t m_head_to_thresholds;
  // the top of the stack is the current wto head
  std::vector<basic_block_label_t> m_stack;

  typedef typename CFG::basic_block_t basic_block_t;
  typedef typename CFG::number_t number_t;
  typedef typename CFG::varname_t varname_t;
  typedef typename CFG::variable_t variable_t;
  typedef ikos::linear_expression<number_t, varname_t> linear_expression_t;
  typedef ikos::linear_constraint<number_t, varname_t> linear_constraint_t;
  typedef ikos::bound<number_t> bound_t;

  typedef crab::cfg::assume_stmt<number_t, varname_t> assume_t;
  // typedef crab::cfg::select_stmt<number_t,varname_t> select_t;
  // typedef crab::cfg::assignment<number_t,varname_t>  assign_t;

  void extract_bounds(const linear_expression_t &e, bool is_strict,
                      std::vector<number_t> &lb_bounds,
                      std::vector<number_t> &ub_bounds) const {
    if (e.size() == 1) {
      auto const &kv = *(e.begin());
      number_t coeff = kv.first;
      variable_t var = kv.second;
      number_t k = -e.constant();
      if (coeff > 0) {
        // e is c*var <= k and c > 0  <---> var <= k/coeff
        ub_bounds.push_back(!is_strict ? k / coeff : (k / coeff) - 1);
        return;
      } else if (coeff < 0) {
        // e is c*var <= k  and c < 0 <---> var >= k/coeff
        lb_bounds.push_back(!is_strict ? k / coeff : (k / coeff) + 1);
        return;
      }
    }
  }

  void get_thresholds(const basic_block_t &bb, thresholds_t &thresholds) const {

    std::vector<number_t> lb_bounds, ub_bounds;
    for (auto const &i : boost::make_iterator_range(bb.begin(), bb.end())) {
      if (i.is_assume()) {
        auto a = static_cast<const assume_t *>(&i);
        linear_constraint_t cst = a->constraint();
        if (cst.is_inequality() || cst.is_strict_inequality()) {
          extract_bounds(cst.expression(), cst.is_strict_inequality(),
                         lb_bounds, ub_bounds);
        }
      }
      // else if (i.is_select()) {
      //   auto s = static_cast<const select_t*>(&i);
      //   linear_constraint_t cst = s->cond();
      //   if (cst.is_inequality() || cst.is_strict_inequality()) {
      //     extract_bounds(cst.expression(), cst.is_strict_inequality(),
      // 		      lb_bounds, ub_bounds);
      //   }
      // }
    }

    // Assuming that the variable is incremented/decremented by
    // some constant k, then we want to adjust the threshold to
    // +/- k so that we have more chance to stabilize in one
    // iteration after widening has been applied.
    int k = 1;
    for (auto n : lb_bounds) {
      thresholds.add(bound_t(n - k));
    }

    for (auto n : ub_bounds) {
      thresholds.add(bound_t(n + k));
    }
  }

public:
  wto_thresholds(CFG cfg, size_t max_size) : m_cfg(cfg), m_max_size(max_size) {}

  void visit(wto_vertex_t &vertex) {
    if (m_stack.empty())
      return;

    basic_block_label_t head = m_stack.back();
    auto it = m_head_to_thresholds.find(head);
    if (it != m_head_to_thresholds.end()) {
      thresholds_t &thresholds = it->second;
      typename CFG::basic_block_t &bb = m_cfg.get_node(vertex.node());
      get_thresholds(bb, thresholds);
    } else {
      CRAB_ERROR("No head found while gathering thresholds");
    }
  }

  void visit(wto_cycle_t &cycle) {
    thresholds_t thresholds(m_max_size);
    typename CFG::basic_block_t &bb = m_cfg.get_node(cycle.head());
    get_thresholds(bb, thresholds);

#if 1
    // XXX: if we want to consider constants from loop
    // initializations
    for (auto pre : boost::make_iterator_range(bb.prev_blocks())) {
      if (pre != cycle.head()) {
        typename CFG::basic_block_t &pred_bb = m_cfg.get_node(pre);
        get_thresholds(pred_bb, thresholds);
      }
    }
#endif

    m_head_to_thresholds.insert(std::make_pair(cycle.head(), thresholds));
    m_stack.push_back(cycle.head());
    for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end();
         ++it) {
      it->accept(this);
    }
    m_stack.pop_back();
  }

  const thresholds_map_t &get_thresholds_map() const {
    return m_head_to_thresholds;
  }

  void write(crab::crab_os &o) const {
    for (auto &kv : m_head_to_thresholds) {
      o << crab::cfg_impl::get_label_str(kv.first) << "=" << kv.second << "\n";
    }
  }

}; // class wto_thresholds

template <typename CFG>
inline crab::crab_os &operator<<(crab::crab_os &o,
                                 const wto_thresholds<CFG> &t) {
  t.write(o);
  return o;
}

} // end namespace iterators
} // end namespace crab


//------------------------------------------------------------------------------
// \file 1_tests.cpp
//------------------------------------------------------------------------------
#include "Algebra/Categories/Category.h"
#include "Algebra/Categories/FiniteCategories/1.h"

#include <boost/test/unit_test.hpp>
#include <cmath>

using Algebra::Categories::Category::identity_morphism;
using Algebra::Categories::FiniteCategories::Category1Object;
using Algebra::Categories::FiniteCategories::Objects1Element;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(FiniteCategories)
BOOST_AUTO_TEST_SUITE(FiniteCategories_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(Category1ObjectAsItsOwnClass)
{
  const Category1Object A;

  BOOST_TEST(A.name() == "A");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(Category1ObjectElementAsAClass)
{
  const Objects1Element A {Objects1Element::ObjectName::A};

  BOOST_TEST(A.name() == 'A');
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IdentityMorphismWorksOnCategory1Object)
{
  const Category1Object A;

  BOOST_TEST_REQUIRE(A.name() == "A");

  const Category1Object& another_A {identity_morphism(A)};

  BOOST_TEST(another_A.name() == "A");
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IdentityMorphismWorksOnCategory1ObjectElement)
{
  const Objects1Element A {Objects1Element::ObjectName::A};

  BOOST_TEST_REQUIRE(A.name() == 'A');

  const auto& another_A = identity_morphism(A);

  BOOST_TEST(another_A.name() == 'A');
}

BOOST_AUTO_TEST_SUITE_END() // FiniteCategories_tests
BOOST_AUTO_TEST_SUITE_END() // FiniteCategories
BOOST_AUTO_TEST_SUITE_END() // Categories


#pragma once

#include "definitions.hpp"
#include "vector.hpp"

#include <Eigen/Dense>

using System = Matrix;

namespace Distance
{
void init(const System&);
Real probe(const System&, int i, int j);
void invalidate_cache(int i);

// The plain, non-memoized euclidian norm.
// Fall back to this if not initialized, or when cache is invalidated.
inline Real distance(const System& system, int i, int j)
{
    return norm(system.row(i) - system.row(j));
}

}  // namespace Distance


#include <iostream>
#include <stdexcept>
#include <vector>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "arf_trajectory/trajectory.h"

const double TOLERANCE = 1e-8;
using Transform = Eigen::Isometry3d;
void comparePoses(const Transform& Ta, const Transform& Tb);

TEST(Number, constructor)
{
    Number x(3.0);
    Number y;
}

TEST(Number, casting)
{
    Number x(3.0);
    double y = x;
    EXPECT_EQ(y, 3.0);
}

TEST(TolerancedNumber, constructor)
{
    TolerancedNumber x(0.5, -1, 2);
    TolerancedNumber y(0.5, -1, 2, 6);
}

TEST(TolerancedNumber, wrongInput)
{
    try
    {
        TolerancedNumber x(0, 1, -1);
    }
    catch (std::invalid_argument const& err)
    {
        EXPECT_EQ(err.what(), std::string("Lower bound should be strictly smaller than upper bound."));
    }
    try
    {
        TolerancedNumber x(-2, -1, 1);
    }
    catch (std::invalid_argument const& err)
    {
        EXPECT_EQ(err.what(), std::string("Nominal value should be withing bounds."));
    }
    try
    {
        TolerancedNumber x(5, -1, 1);
    }
    catch (std::invalid_argument const& err)
    {
        EXPECT_EQ(err.what(), std::string("Nominal value should be withing bounds."));
    }
}

TEST(TrajectoryPoint, constructor)
{
    TolerancedNumber x(1, 1, 2, 2);
    Number y(5);
    TolerancedNumber z(1, 1, 3, 3);
    Number rx, ry, rz;
    TrajectoryPoint tp(x, y, z, rx, ry, rz);
}

TEST(TrajectoryPoint, gridSampling)
{
    TolerancedNumber x(1, 1, 2, 2);
    Number y(5);
    TolerancedNumber z(1, 1, 3, 3);
    Number rx, ry, rz;
    TrajectoryPoint tp(x, y, z, rx, ry, rz);

    std::vector<Transform> actual;
    actual = tp.getGridSamples();

    std::size_t N = actual.size();

    std::vector<Transform> expected;
    expected.resize(N);
    for (std::size_t i = 0; i < N; ++i)
        expected[i] = Eigen::Isometry3d::Identity();
    expected[0].translation() << 1, 5, 1;
    expected[1].translation() << 1, 5, 2;
    expected[2].translation() << 1, 5, 3;
    expected[3].translation() << 2, 5, 1;
    expected[4].translation() << 2, 5, 2;
    expected[5].translation() << 2, 5, 3;

    for (std::size_t i = 0; i < N; ++i)
        comparePoses(expected[i], actual[i]);
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

void comparePoses(const Transform& Ta, const Transform& Tb)
{
    using Matrix = Eigen::Matrix3d;
    using Vector = Eigen::Vector3d;

    Matrix Ra = Ta.rotation(), Rb = Tb.rotation();
    for (int i = 0; i < Ra.rows(); ++i)
    {
        for (int j = 0; j < Ra.cols(); ++j)
        {
            EXPECT_NEAR(Ra(i, j), Rb(i, j), TOLERANCE);
        }
    }

    Vector pa = Ta.translation(), pb = Tb.translation();
    EXPECT_NEAR(pa[0], pb[0], TOLERANCE);
    EXPECT_NEAR(pa[1], pb[1], TOLERANCE);
    EXPECT_NEAR(pa[2], pb[2], TOLERANCE);
}