/**
 *    \file include/geombd/CRTP/JointDerived/Visitors.hxx
 *    \author Alvaro Paz, Gustavo Arechavaleta
 *    \version 1.0
 *    \date 2021
 *
 *    Classes for visitors
 *    Copyright (c) 2021 Cinvestav
 *    This library is distributed under the MIT License.
 */

#ifndef GEOMBD_VISITORS_HXX
#define GEOMBD_VISITORS_HXX

#define EIGEN_NO_DEBUG
#include "Eigen/Core"

//#include <boost/foreach.hpp>
//#include <boost/shared_ptr.hpp>
#include <boost/variant.hpp>

namespace geoCRTP{

  //! Forward Kinematics Visitor
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Matrix3Type>
  class FwdKin_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    FwdKin_visitor( ) : R(nullptr), S(nullptr) {}

    ScalarType qi;
    Vector3Type* S;
    Matrix3Type* R;

    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
        BaseType.FwdKin(qi, (*S).derived(), (*R).derived());
      return 0;
    }

  };

  //! Visitor for Twist and bias elements at root
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Vector6Type, typename Matrix6Type>
  class TCP_root_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    TCP_root_visitor( ) : S(nullptr), S_i(nullptr), p_(nullptr), M_(nullptr) {}

    //! Members
    ScalarType vi;  Vector3Type* S;  Vector6Type* S_i;  Vector6Type* p_;  Matrix6Type* M_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
        BaseType.TCP_root(vi, (*S).derived(), (*S_i).derived(), (*p_).derived(), (*M_).derived());
      return 0;
    }

  };

  //! Visitor for Twist and bias elements.
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Matrix3Type, typename Matrix6Type,
           typename Vector3Type, typename Vector6Type>
  class TwCbPb_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    TwCbPb_visitor( ) : S(nullptr), R_(nullptr), P_(nullptr), S_j(nullptr),
                        M_(nullptr), S_i(nullptr), c_(nullptr), p_(nullptr) {}

    //! Members
    bool zeroFlag;
    ScalarType vi;    Vector3Type* S;    Matrix3Type* R_;   Vector3Type* P_;  Vector6Type* S_j;
    Matrix6Type* M_;  Vector6Type* S_i;  Vector6Type* c_;   Vector6Type* p_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      BaseType.TwCbPb(zeroFlag, vi, (*S).derived(), (*R_).derived(),  (*P_).derived(), (*S_j).derived(), (*M_).derived(),
                      (*S_i).derived(), (*c_).derived(), (*p_).derived());
      return 0;
    }

  };

  //! Visitor for U, u & invD expressions
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Vector6Type, typename Matrix6Type>
  class UuiD_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    UuiD_visitor( ) : u(nullptr), iD(nullptr), S(nullptr), U_(nullptr), P_A_(nullptr), M_A_(nullptr) {}

    //! Members
    ScalarType* u;    ScalarType* iD;     ScalarType tau;     Vector3Type* S;
    Vector6Type* U_;  Vector6Type* P_A_;  Matrix6Type* M_A_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      ScalarType & u_ = (*u);  ScalarType & iD_ = (*iD);
      BaseType.UuiD(u_, iD_, tau, (*S).derived(), (*U_).derived(), (*P_A_).derived(), (*M_A_).derived());
      return 0;
    }

  };

  //! Visitor for preparing inertial expressions
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Vector6Type, typename Matrix6Type>
  class PreIner_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    PreIner_visitor( ) : S(nullptr), U_(nullptr), c_(nullptr), P_a_(nullptr),
                         M_a_(nullptr), P_A_(nullptr), M_A_(nullptr) {}

    //! Members
    ScalarType u, iD;   Vector3Type* S;
    Vector6Type* U_;    Vector6Type* c_;    Vector6Type* P_a_;
    Matrix6Type* M_a_;  Vector6Type* P_A_;  Matrix6Type* M_A_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      BaseType.PreIner(u, iD, (*S).derived(), (*U_).derived(), (*c_).derived(), (*P_a_).derived(),
                       (*M_a_).derived(), (*P_A_).derived(), (*M_A_).derived());
      return 0;
    }

  };

  //! Visitor for inertial back projection
  //!------------------------------------------------------------------------------!//
  template<typename Vector3Type, typename Matrix3Type, typename Vector6Type, typename Matrix6Type>
  class InerProj_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    InerProj_visitor( ) : S(nullptr), P_(nullptr), R_(nullptr), P_a_(nullptr), P_A_(nullptr), M_a_(nullptr), M_A_(nullptr) {}

    //! Members
    bool P_z;           Vector3Type* S;
    Vector3Type* P_;    Matrix3Type* R_;
    Vector6Type* P_a_;  Vector6Type* P_A_;
    Matrix6Type* M_a_;  Matrix6Type* M_A_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      BaseType.InerProj(P_z, (*S).derived(), (*P_).derived(), (*R_).derived(), (*P_a_).derived(), (*P_A_).derived(),
                        (*M_a_).derived(), (*M_A_).derived());
      return 0;
    }

  };

  //! Visitor for computing the acceleration
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Matrix3Type, typename Vector6Type>
  class Accel_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    Accel_visitor( ) : ddq(nullptr), S_(nullptr), P_(nullptr), R_(nullptr), c_(nullptr),
                       U_(nullptr), Acc_i_(nullptr), Acc_j_(nullptr) {}

    //! Members
    bool zeroFlag;
    ScalarType u, iD;
    ScalarType* ddq;

    Vector3Type* S_;  Vector3Type* P_;  Matrix3Type* R_;  Vector6Type* c_;
    Vector6Type* U_;  Vector6Type* Acc_i_;  Vector6Type* Acc_j_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      BaseType.Accel(zeroFlag, u, iD, ddq, (*S_).derived(), (*P_).derived(), (*R_).derived(), (*c_).derived(),
                     (*U_).derived(), (*Acc_i_).derived(), (*Acc_j_).derived());
      return 0;
    }

  };

  //! Visitor for computing the acceleration at root
  //!------------------------------------------------------------------------------!//
  template<typename ScalarType, typename Vector3Type, typename Matrix3Type, typename Vector6Type>
  class Accel_root_visitor : public boost::static_visitor<int> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    //! Constructor
    Accel_root_visitor( ) : ddq(nullptr), S_(nullptr), P_(nullptr), R_(nullptr), U_(nullptr), Acc_i_(nullptr) {}

    //! Members
    ScalarType u, iD;
    ScalarType* ddq;

    Vector3Type* S_;  Vector3Type* P_;  Matrix3Type* R_;  Vector6Type* U_;  Vector6Type* Acc_i_;

    //! Method -> member function
    template<typename Derived>
    int operator()(CRTPInterface<Derived> & BaseType) const {
      BaseType.AccelRoot(u, iD, ddq, (*S_).derived(), (*P_).derived(), (*R_).derived(), (*U_).derived(), (*Acc_i_).derived());
      return 0;
    }

  };

}

#endif // GEOMBD_VISITORS_HXX


/**
 * Content
 * An optimizated network graph class
 *
 * The optimization is achieved by recording the output
 * of routing in two variables predecessor map and distance map
 * and regularly updating and cleaning them.
 *
 * It avoids intialization of distances and predecessors vector
 * in each iteration of the driving distance calculation. With a
 * large road network, a small proportion of their nodes are visited
 * in the precomputation. 2018.03.09
 *
 * Add a property map for vertices in the graph to store discontinuous ID
 * for nodes.
 *
 * @author: Can Yang
 * @version: 2018.03.09
 */
#ifndef MM_NETWORK_GRAPH_OPT_HPP
#define MM_NETWORK_GRAPH_OPT_HPP
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <deque>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>
#include "types.hpp"
#include "reader.hpp"
#include "float.h"
#include "network.hpp"
#include <algorithm> // std::reverse
#include <unordered_map>
#include <boost/archive/binary_oarchive.hpp> // Binary output of UBODT
namespace MM{
class NetworkGraphOpt
{
public:
    // A infinity value used in the routing algorithm
    static constexpr double DIST_NOT_FOUND = DBL_MAX;
    /**
     *  Construct a network graph from a network object
     */
    NetworkGraphOpt(Network *network) {
        std::vector<Edge> *edges = network->get_edges();
        std::cout << "Construct graph from network edges start" << '\n';
        // Key is the external ID and value is the index of vertice
        std::unordered_map<int,int> vertex_map;
        int current_idx=-1;
        edge_descriptor e;
        bool inserted;
        g = Graph_T(); //18
        int N = edges->size();
        int source_idx = 0;
        int target_idx = 0;
        printf("Network edges :%d \n", N);
        for (int i = 0; i < N; ++i) {
            Edge &network_edge = (*edges)[i];
            auto search = vertex_map.find(network_edge.source);
            // Search for source node idx
            if(search != vertex_map.end()) {
                // A node exists already
                source_idx = search->second;
            } else {
                // A new node is found
                ++current_idx;
                vertex_map.insert({network_edge.source,current_idx});
                source_idx = current_idx;
                vertex_id_vec.push_back(network_edge.source);
            };
            // Search for target node idx
            search = vertex_map.find(network_edge.target);
            if(search != vertex_map.end()) {
                // A node exists already
                target_idx = search->second;
            } else {
                // A new node is found
                ++current_idx;
                vertex_map.insert({network_edge.target,current_idx});
                target_idx = current_idx;
                vertex_id_vec.push_back(network_edge.target);
            };
            // boost::tie(e, inserted) = add_edge(network_edge.source, network_edge.target, g);
            boost::tie(e, inserted) = add_edge(source_idx,target_idx, g);
            // id is the FID read, id_attr is the external property in SHP
            g[e].id = network_edge.id;
            g[e].length = network_edge.length;
            //printf( "Edge read %d,%d,%d,%lf\n",network_edge.id,network_edge.source,network_edge.target,network_edge.length);
        }
        num_vertices = boost::num_vertices(g);
        std::cout << "Graph nodes " << num_vertices << '\n';
        initialize_distances_predecessors();
        std::cout << "Construct graph from network edges end" << '\n';
    };
    /**
     * Precompute an UBODT with delta and save it to the file
     * @param filename [description]
     * @param delta    [description]
     */
    void precompute_ubodt(const std::string &filename, double delta, bool binary=true) {
        int step_size = num_vertices/10;
        if (step_size<10) step_size=10;
        std::ofstream myfile(filename);
        std::cout << "Start to generate UBODT with delta " << delta << '\n';
        std::cout << "Output format " << (binary?"binary":"csv") << '\n';
        if (binary){
            boost::archive::binary_oarchive oa(myfile);
            vertex_iterator vi, vend;
            for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
                if (*vi%step_size==0) std::cout<<"Progress "<<*vi<< " / " << num_vertices <<'\n';
                driving_distance_binary(*vi, delta, oa);
            }
        } else {
            myfile << "source;target;next_n;prev_n;next_e;distance\n";
            vertex_iterator vi, vend;
            for (boost::tie(vi, vend) = vertices(g); vi != vend ; ++vi) {
                if (*vi%step_size==0) std::cout<<"Progress "<<*vi<< " / " << num_vertices <<'\n';
                driving_distance_csv(*vi, delta, myfile);
            }
        }
        myfile.close();
    };
private:
    /* Type definition for the property stored at each edge */
    struct Edge_Property
    {
        int id;
        double length;
    };
    // Boost graph type definition
    typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::directedS, boost::no_property, Edge_Property> Graph_T;
    typedef Graph_T::vertex_descriptor vertex_descriptor;
    typedef Graph_T::edge_descriptor edge_descriptor;
    typedef boost::graph_traits<Graph_T>::vertex_iterator vertex_iterator;
    typedef boost::graph_traits<Graph_T>::out_edge_iterator out_edge_iterator;
    struct found_goals {}; // Used for driving distances
    /**
     * The visitor is an inner class whose function examine_vertex()
     * is called whenever a new node is found in conventional Dijkstra
     * algorithm.
     *
     * It is called in the driving_distance function.
     */
    class driving_distance_visitor : public boost::default_dijkstra_visitor {
    public:
        // Create a visitor
        explicit driving_distance_visitor(
            double distance_goal,
            std::deque< vertex_descriptor > &nodesInDistance,
            std::vector< double > &distances,
            std::vector< vertex_descriptor > &examined_vertices_ref
        ) : m_distance_goal(distance_goal), m_nodes(nodesInDistance), m_dist(distances),
        m_examined_vertices(examined_vertices_ref) {};
        template <class Graph>void examine_vertex(vertex_descriptor u, Graph &g) {
            DEBUG (2) std::cout << "Examine node " << u << '\n';
            m_nodes.push_back(u);
            if (m_dist[u] > m_distance_goal) {
                m_nodes.pop_back();
                throw found_goals();
            }
        };
        template <class Graph>void edge_relaxed(edge_descriptor e, Graph &g) {
            // Add v to the examined vertices
            DEBUG (2) std::cout << "Examine edge" << e << '\n';
            m_examined_vertices.push_back(boost::target(e, g));
        };
    private:
        double m_distance_goal; //Delta
        std::deque< vertex_descriptor > &m_nodes; //Precedessors
        std::vector< double > &m_dist; // Distances
        std::vector< vertex_descriptor > & m_examined_vertices; //Examined nodes
    }; // driving_distance_visitor

    Graph_T g; // The member storing a boost graph
    /**
     *  Find the edge ID given a pair of nodes and its cost
     */
    int get_edge_id(vertex_descriptor source, vertex_descriptor target, double cost) {
        edge_descriptor e;
        out_edge_iterator out_i, out_end;
        for (boost::tie(out_i, out_end) = boost::out_edges(source, g);
                out_i != out_end; ++out_i) {
            e = *out_i; // Can we directly get edge id here or latter from the graph
            if (target == boost::target(e, g) && (g[e].length - cost <= DOUBLE_MIN)) {
                return  g[e].id;
            }
        }
        std::cout << "Edge not found for source " << source << " target " << target
                  << " cost " << cost << '\n';
        return -1;
    };

    /**
     *  Get the successors (next node visited) for each node in a
     *  shortest path tree defined by a deque and a predecessor vector
     */
    std::vector<vertex_descriptor> get_successors(std::deque<vertex_descriptor> &nodesInDistance, std::vector<vertex_descriptor>& predecessors) {
        int N = nodesInDistance.size();
        std::vector<vertex_descriptor> successors = std::vector<vertex_descriptor>(N);
        int i;
        vertex_descriptor u, v;
        vertex_descriptor source = nodesInDistance[0];// source node
        for (i = 0; i < N; ++i) {
            v = nodesInDistance[i];
            while ((u = predecessors[v]) != source) {
                v = u;
            }
            successors[i] = v;
        }
        return successors;
    };
    /**
     * Given a source node and an upper bound distance delta
     * write the UBODT rows to a file
     */
    void driving_distance_csv(const vertex_descriptor& source, double delta, std::ostream& stream) {
        DEBUG (2) std::cout << "Debug progress source " << source << '\n';
        std::deque<vertex_descriptor> nodesInDistance;
        examined_vertices.push_back(source);
        double inf = std::numeric_limits<double>::max();
        distances_map[source]=0;
        // In BGL, http://www.boost.org/doc/libs/1_66_0/boost/graph/dijkstra_shortest_paths_no_color_map.hpp
        // The named parameter version is only defined for dijkstra_shortest_paths_no_color_map
        // Therefore, we need to explicitly pass in the arguments
        // boost::choose_param choose parameter with default value
        try {
            // This part to be fixed
            dijkstra_shortest_paths_no_color_map_no_init(
                g,
                source,
                //boost::predecessor_map(&predecessors_map[0]),
                make_iterator_property_map(predecessors_map.begin(),get(boost::vertex_index, g),predecessors_map[0]),
                //boost::distance_map(boost::make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g))),
                //&distances_map[0],
                make_iterator_property_map(distances_map.begin(),get(boost::vertex_index, g),distances_map[0]),
                //make_iterator_property_map(distances_map.begin(), boost::vertex_index_map(get(boost::vertex_index, g)),distances_map[0]),
                //boost::distance_map(boost::make_iterator_property_map(distances_map.begin(), get(boost::vertex_index, g))),
                get(&Edge_Property::length, g),
                get(boost::vertex_index, g),
                std::less<double>(), //DistanceCompare distance_compare,
                boost::closed_plus<double>(inf),
                inf,
                0,
                driving_distance_visitor(
                    delta, nodesInDistance, distances_map, examined_vertices
                )
            );
        } catch (found_goals& goal) {
            //std::cout << "Found goals" << '\n';
        }
        // Get successors for each node reached
        DEBUG (2) std::cout << "Find nodes in distance # "<< nodesInDistance.size() <<'\n';
        std::vector<vertex_descriptor> successors =
            get_successors(nodesInDistance, predecessors_map);
        double cost;
        int edge_id;
        int k = 0;
        vertex_descriptor node;
        std::stringstream node_output_buf;
        while (k < nodesInDistance.size()) {
            node = nodesInDistance[k];
            if (source != node) {
                // The cost is need to identify the edge ID
                cost = distances_map[successors[k]] - distances_map[source];
                edge_id = get_edge_id(source, successors[k], cost);
                stream << vertex_id_vec[source] << ";" << vertex_id_vec[node] << ";" << vertex_id_vec[successors[k]] << ";"
                       << vertex_id_vec[predecessors_map[node]] << ";" << edge_id << ";" << distances_map[node]
                       << "\n";
                // stream << source << ";" << node << ";" << successors[k] << ";"
                //        << predecessors_map[node] << ";" << edge_id << ";" << distances_map[node]
                //        << "\n";
            }
            ++k;
        }
        DEBUG (2) std::cout << "Clean examined vertices"<< examined_vertices.size() <<'\n';
        clean_distances_predecessors();
    };
    void driving_distance_binary(const vertex_descriptor& source, double delta, boost::archive::binary_oarchive& oa) {
        DEBUG (2) std::cout << "Debug progress source " << source << '\n';
        std::deque<vertex_descriptor> nodesInDistance;
        examined_vertices.push_back(source);
        double inf = std::numeric_limits<double>::max();
        distances_map[source]=0;
        try {
            // This part to be fixed
            dijkstra_shortest_paths_no_color_map_no_init(
                g,
                source,
                make_iterator_property_map(predecessors_map.begin(),get(boost::vertex_index, g),predecessors_map[0]),
                make_iterator_property_map(distances_map.begin(),get(boost::vertex_index, g),distances_map[0]),
                get(&Edge_Property::length, g),
                get(boost::vertex_index, g),
                std::less<double>(), //DistanceCompare distance_compare,
                boost::closed_plus<double>(inf),
                inf,
                0,
                driving_distance_visitor(
                    delta, nodesInDistance, distances_map, examined_vertices
                )
            );
        } catch (found_goals& goal) {
            //std::cout << "Found goals" << '\n';
        }
        // Get successors for each node reached
        DEBUG (2) std::cout << "Find nodes in distance # "<< nodesInDistance.size() <<'\n';
        std::vector<vertex_descriptor> successors =get_successors(nodesInDistance, predecessors_map);
        double cost;
        int edge_id;
        int k = 0;
        vertex_descriptor node;
        while (k < nodesInDistance.size()) {
            node = nodesInDistance[k];
            if (source != node) {
                // The cost is need to identify the edge ID
                cost = distances_map[successors[k]] - distances_map[source];
                edge_id = get_edge_id(source, successors[k], cost);
                oa << vertex_id_vec[source];
                oa << vertex_id_vec[node];
                oa << vertex_id_vec[successors[k]];
                oa << vertex_id_vec[predecessors_map[node]];
                oa << edge_id;
                oa << distances_map[node];
            }
            ++k;
        }
        DEBUG (2) std::cout << "Clean examined vertices"<< examined_vertices.size() <<'\n';
        clean_distances_predecessors();
    };
    /*
       Clean the distance map and predecessor map
     */
    void initialize_distances_predecessors(){
        // Need initialization
        predecessors_map= std::vector<vertex_descriptor>(num_vertices);
        distances_map = std::vector<double>(num_vertices);
        for (int i = 0; i < num_vertices; ++i) {
            distances_map[i] = std::numeric_limits<double>::max();
            predecessors_map[i] = i;
        }

    }
    void clean_distances_predecessors(){
        // Update the properties of examined nodes
        int N = examined_vertices.size();
        for (int i = 0; i < N; ++i) {
            vertex_descriptor v = examined_vertices[i];
            distances_map[v] = std::numeric_limits<double>::max();
            predecessors_map[v] = v;
        }
        examined_vertices.clear();
        // Clear the examined vertices
    };
    static constexpr double DOUBLE_MIN = 1.e-6; // This is used for comparing double values
    // Two maps record the routing output
    std::vector<vertex_descriptor> predecessors_map;
    // a list of costs stored for one node to all nodes in the graph
    std::vector<double> distances_map;
    std::vector<int> vertex_id_vec; // stores the external ID of each vertex in G
    std::vector<vertex_descriptor> examined_vertices; // Nodes whose distance in the dist_map is updated.
    int num_vertices=0;
}; // NetworkGraphOpt
} // MM
#endif /* MM_NETWORK_GRAPH_OPT_HPP */


#include <iostream>

//--- SpatialOps includes ---//
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/structured/IntVec.h>
#include <spatialops/structured/MemoryWindow.h>
#include <spatialops/structured/stencil/FVStaggeredOperatorTypes.h>
#include <spatialops/OperatorDatabase.h>
#include <spatialops/structured/stencil/StencilBuilder.h>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/Nebo.h>

//-- boost includes ---//
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace po = boost::program_options;

using namespace SpatialOps;

int print_length = 10;

template<typename Field>
void print(Field const & given) {
  typename Field::const_iterator ig = given.begin();

  for(int i = 0; i < print_length; i++) {
    std::cout << *ig << std::endl;
    ++ig;
  };

  std::cout << std::endl;
};

template<typename Field>
void print_all(Field const & given) {
  typename Field::const_iterator ig = given.begin();

  MemoryWindow window = given.window_with_ghost();
  for(int kk = 0; kk < window.glob_dim(2); kk++) {
      for(int jj = 0; jj < window.glob_dim(1); jj++) {
          for(int ii = 0; ii < window.glob_dim(0); ii++, ++ig) {
              std::cout << *ig << " ";
          }
          std::cout <<std::endl;
      }
      std::cout <<std::endl;
  };
};

#define RUN_TEST(TEST,							\
		 TYPE)							\
  boost::posix_time::ptime start( boost::posix_time::microsec_clock::universal_time() ); \
  boost::posix_time::ptime end( boost::posix_time::microsec_clock::universal_time() ); \
  int ii = 0;								\
  start = boost::posix_time::microsec_clock::universal_time();		\
									\
  for(; ii < number_of_runs; ii++) {					\
    TEST;								\
  };									\
									\
  end = boost::posix_time::microsec_clock::universal_time();		\
  std::cout << TYPE;							\
  std::cout << " runs: ";						\
  std::cout << number_of_runs;						\
  std::cout << " result: ";						\
  std::cout << (end - start).total_microseconds()*1e-6;			\
  std::cout << std::endl;

int main(int iarg, char* carg[]) {
    typedef SVolField Field;

    bool test;
    int nx, ny, nz;
    int number_of_runs;
    double Lx, Ly, Lz;
#ifdef ENABLE_THREADS
  int thread_count;
#endif

    // parse the command line options input describing the problem
    {
        po::options_description desc("Supported Options");
	desc.add_options()
	  ( "help", "print help message" )
	  ( "nx", po::value<int>(&nx)->default_value(10), "Grid in x" )
	  ( "ny", po::value<int>(&ny)->default_value(10), "Grid in y" )
	  ( "nz", po::value<int>(&nz)->default_value(10), "Grid in z" )
	  ( "Lx", po::value<double>(&Lx)->default_value(1.0),"Length in x")
	  ( "Ly", po::value<double>(&Ly)->default_value(1.0),"Length in y")
	  ( "Lz", po::value<double>(&Lz)->default_value(1.0),"Length in z")
	  ( "check", po::value<bool>(&test)->default_value(true),"Compare results of old and new versions")
#ifdef ENABLE_THREADS
      ( "tc", po::value<int>(&thread_count)->default_value(NTHREADS), "Number of threads for Nebo")
#endif
	  ( "runs", po::value<int>(&number_of_runs)->default_value(1), "Number of iterations of each test");

	po::variables_map args;
	po::store( po::parse_command_line(iarg,carg,desc), args );
	po::notify(args);

	if (args.count("help")) {
	    std::cout << desc << "\n";
	    return 1;
	}

#ifdef ENABLE_THREADS
    set_hard_thread_count(thread_count);
#endif
    }

    const int nghost = 1;
    const GhostData ghost(nghost);
    const BoundaryCellInfo bc = BoundaryCellInfo::build<Field>(false,false,false);
    const MemoryWindow window( get_window_with_ghost(IntVec(nx,ny,nz),ghost,bc) );

    Field  a( window, bc, ghost, NULL );
    Field  b( window, bc, ghost, NULL );
    Field cr( window, bc, ghost, NULL );
    Field sr( window, bc, ghost, NULL );

    Field::iterator ia = a.begin();
    Field::iterator ib = b.begin();
    for(size_t kk = 0; kk < window.glob_dim(2); kk++) {
        for(size_t jj = 0; jj < window.glob_dim(1); jj++) {
            for(size_t ii = 0; ii < window.glob_dim(0); ii++, ++ia, ++ib) {
	      *ia = ii + jj * 2 + kk * 4;
	      *ib = ii + jj * 3 + kk * 5;
            }
        }
    };

  if( dim[0]>1 && dim[1]>1 && dim[2]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter3DStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[0]>1 && dim[1]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter2DXYStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[0]>1 && dim[2]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter2DXZStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[1]>1 && dim[2]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter2DYZStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[0]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter1DXStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[1]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter1DYStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  } else if( dim[2]>1 ) {
      const NeboAverageStencilBuilder<BoxFilter1DZStencilCollection::StPtCollection, SVolField, SVolField> filter;
      RUN_TEST(filter.apply_to_field( a, cr ), "filter");
  }

    return 0;
};


//  Copyright 2015 Stephan Menzel. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#pragma once
#include "MooseToolsConfig.hpp"

#include <boost/cstdint.hpp>
#include <boost/uuid/uuid.hpp>

namespace moose {
namespace tools {

/*! @brief thread safe random number shortcut
 * This will give you a thread safe random number between 0 and n_max
 *  @throw std::bad_alloc when out of memory on first use
 */
MOOSE_TOOLS_API boost::uint64_t urand(const boost::uint64_t n_max);

/*! @brief thread safe random number shortcut
 *  This will give you a thread safe random number between n_min and n_max
 *  @note asserts when n_min >= n_max
 *  @throw std::bad_alloc when out of memory on first use
 */
MOOSE_TOOLS_API boost::uint64_t urand(const boost::uint64_t n_min, const boost::uint64_t n_max);

/*! @brief thread safe random number shortcut
 * This will give you a thread safe random number between 0 and std::numerical_limits<uint64_t>::max()
 *  @throw std::bad_alloc when out of memory on first use
 */
MOOSE_TOOLS_API boost::uint64_t urand();

/*! @brief creates a random uuid out of thin air
 *  @return random uuid
 *  @throw std::bad_alloc when out of memory on first use
 */
MOOSE_TOOLS_API boost::uuids::uuid ruuid();

/*! @brief supposedly the fasted.
	Found this at https://stackoverflow.com/questions/1640258/need-a-fast-random-generator-for-c
	and just took it here. All credits to original author
 */
MOOSE_TOOLS_API unsigned long xorshf96();

}
}


/*!
 * @file
 * This file defines `boost::mpl::set_intersection`.
 */

#ifndef BOOST_MPL_SET_INTERSECTION_HPP
#define BOOST_MPL_SET_INTERSECTION_HPP

#include <boost/mpl/clear.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/placeholders.hpp>

namespace boost { namespace mpl {
    template <typename Set1, typename Set2, typename ...Setn>
    struct set_intersection
        : set_intersection<
            typename set_intersection<Set1, Set2>::type,
            Setn...
        >
    { };

    template <typename Set1, typename Set2>
    struct set_intersection<Set1, Set2>
        : fold<
            Set1,
            typename clear<Set1>::type,
            if_<has_key<Set2, _2>, insert<_1, _2>, _1>
        >
    { };
}}

#endif // !BOOST_MPL_SET_INTERSECTION_HPP


/*!
 * @file
 * Defines `boost::mpl11::detail::left_folds::bounded`.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

//////////////////////////////////////////////////////////////////////////
// GENERATED HEADER: DO NOT EDIT.
//////////////////////////////////////////////////////////////////////////

#ifndef BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP
#define BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP

#include <boost/mpl11/detail/std_size_t.hpp>
#include <boost/mpl11/fwd/iterable.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace left_folds {

    template <
        std_size_t n, template <typename ...> class f,
        typename state, typename xs>
    struct bounded_impl {
        using left = bounded_impl<
            (n - 6)/2,
            f,
            f<f<f<f<f<f<state, head<xs>>, head<tail<xs>>>, head<tail<tail<xs>>>>, head<tail<tail<tail<xs>>>>>, head<tail<tail<tail<tail<xs>>>>>>, head<tail<tail<tail<tail<tail<xs>>>>>>>,
            tail<tail<tail<tail<tail<tail<xs>>>>>>
        >;
        using right = bounded_impl<
            (n - 6) - (n - 6)/2,
            f,
            typename left::result,
            typename left::rest
        >;

        using rest = typename right::rest;
        using result = typename right::result;
    };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<0, f, state, xs> {
            using rest = xs;
            using result = state;
        };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<1, f, state, xs> {
            using rest = tail<xs>;
            using result = f<state, head<xs>>;
        };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<2, f, state, xs> {
            using rest = tail<tail<xs>>;
            using result = f<f<state, head<xs>>, head<tail<xs>>>;
        };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<3, f, state, xs> {
            using rest = tail<tail<tail<xs>>>;
            using result = f<f<f<state, head<xs>>, head<tail<xs>>>, head<tail<tail<xs>>>>;
        };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<4, f, state, xs> {
            using rest = tail<tail<tail<tail<xs>>>>;
            using result = f<f<f<f<state, head<xs>>, head<tail<xs>>>, head<tail<tail<xs>>>>, head<tail<tail<tail<xs>>>>>;
        };

        template <template <typename ...> class f, typename state, typename xs>
        struct bounded_impl<5, f, state, xs> {
            using rest = tail<tail<tail<tail<tail<xs>>>>>;
            using result = f<f<f<f<f<state, head<xs>>, head<tail<xs>>>, head<tail<tail<xs>>>>, head<tail<tail<tail<xs>>>>>, head<tail<tail<tail<tail<xs>>>>>>;
        };

    /*!
     * @ingroup details
     *
     * Left fold stopping after `n` steps.
     *
     * The metafunction is unlifted for performance.
     */
    template <
        std_size_t n,
        template <typename ...> class f,
        typename state,
        typename xs
    >
    using bounded = typename bounded_impl<n, f, state, xs>::result;
}}}} // end namespace boost::mpl11::detail::left_folds

#endif // !BOOST_MPL11_DETAIL_LEFT_FOLDS_BOUNDED_HPP


#include <iostream>
#include <fstream>
#include <cstdio>
#include <vector>
#include <algorithm>
#include <cmath>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include "wordtrie.h"

int TrieNode_c::num_descendants() const {
  int cnt = 1;
  for(auto iter = m_children.begin(); iter != m_children.end(); iter++)
    cnt += iter->second->num_descendants();
  return cnt;
}

void Trie_c::insert(std::string const& word, long long count) {
  TrieNode_p current = root();
  current->m_count += count;

  for(size_t i=0; i < word.size(); i++) {
    char ch = word[i];
    auto iter = current->m_children.find(ch);
    TrieNode_p node;

    if(iter != current->m_children.end())
      node = iter->second.get();
    else {
      node = new TrieNode_c();
      node->m_parent = current;
      current->m_children[ch].reset(node);
    }

    current = node;
    current->m_count += count;
  }

  current->m_endofword = true;
}

TrieNode_p Trie_c::search(std::string const& word) const {
  TrieNode_p current = root();
  for(size_t i=0; i < word.size(); i++) {
    char ch = word[i];
    auto iter = current->m_children.find(ch);

    if(iter == current->m_children.end())
      return nullptr;
    current = iter->second.get();
  }
  return current;
}

double Trie_c::prob(std::string const& word) const {
  TrieNode_p node = search(word);
  if(!node)
    return 0.0;
  return ((double) node->m_count) / total_count();
}

double Trie_c::cond_prob(std::string const& word) const {
  TrieNode_p node = search(word);
  if(!node)
    return 0.0;
  return ((double) node->m_count) / node->m_parent->m_count;
}

void load(std::istream& is, Trie_c& out_trie) {
  boost::archive::text_iarchive ia(is);
  ia >> out_trie;
}

void save(std::ostream& os, Trie_c const& in_trie) {
  boost::archive::text_oarchive oa(os);
  oa << in_trie;
}

void load_from_file(std::string const& filepath, Trie_c& out_trie) {
  std::ifstream ifs(filepath);
  load(ifs, out_trie);
}

void save_to_file(std::string const& filepath, Trie_c const& in_trie) {
  std::ofstream ofs(filepath);
  save(ofs, in_trie);
}


#include <gtest/gtest.h>
#include "headers/speed_constraints.h"
#include "headers/field_functions.h"
#include <boost/filesystem.hpp>
#include <chrono>

TEST(straight_8_robots_1, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_8_robots_1");

  for (int i = 0; i <= 2000; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("straight_8_robots_1/straight_8_robots_1_" + std::to_string(i) + ".png",
                          "straight_8_robots_1", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_8_robots_2, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_8_robots_2");

  for (int i = 0; i <= 3400; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(0, 1);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("straight_8_robots_2/straight_8_robots_2_" + std::to_string(i) + ".png",
                              "straight_8_robots_2", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                              vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_8_robots_3, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 5.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 5.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_8_robots_3");

  for (int i = 0; i <= 1500; i++)
  {
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("straight_8_robots_3/straight_8_robots_3_" + std::to_string(i) + ".png",
                          "straight_8_robots_3", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(straight_8_robots_4, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 5);
  Robot r7(Position_t(8.0, 15.0), 6);
  Robot r8(Position_t(15.0, 8.0), 7);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);
  boost::add_vertex(r5, *rg);
  boost::add_vertex(r6, *rg);
  boost::add_vertex(r7, *rg);
  boost::add_vertex(r8, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 5; i < 41; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 5.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 5.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("straight_8_robots_4");

  for (int i = 0; i <= 3600; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 59)
    {
      leaderV = Vector_t(0, 1);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printWidePlotWithArrows("straight_8_robots_4/straight_8_robots_4_" + std::to_string(i) + ".png",
                          "straight_8_robots_4", 0, 90, 2, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

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


#ifndef SOLVER_PARAMS_HPP
#define SOLVER_PARAMS_HPP

#include <Eigen/Dense>

namespace ms  // mader solver
{
struct par_solver
{
  ///// Will not change between iterations
  double x_min = -std::numeric_limits<double>::max();
  double x_max = std::numeric_limits<double>::max();

  double y_min = -std::numeric_limits<double>::max();
  double y_max = std::numeric_limits<double>::max();

  double z_min = -std::numeric_limits<double>::max();
  double z_max = std::numeric_limits<double>::max();
  Eigen::Vector3d v_max;
  Eigen::Vector3d a_max;
  Eigen::Vector3d j_max;
  double dc;
  double dist_to_use_straight_guess;
  int a_star_samp_x;
  int a_star_samp_y;
  int a_star_samp_z;
  double a_star_fraction_voxel_size;
  int num_pol;
  int deg_pol;
  double weight;
  double epsilon_tol_constraints;
  double xtol_rel;
  double ftol_rel;
  std::string solver;
  std::string basis;
  double a_star_bias;
  bool allow_infeasible_guess;
  double Ra;

  double alpha_shrink;
};
}  // namespace ms

#endif


// Copyright Aleksey Gurtovoy 2002-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: is_sequence.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/aux_/test.hpp>

template< typename T > struct std_vector
{
    T* begin();
};

MPL_TEST_CASE()
{
    MPL_ASSERT_NOT(( is_sequence< std_vector<int> > ));
    MPL_ASSERT_NOT(( is_sequence< int_<0> > ));
    MPL_ASSERT_NOT(( is_sequence< int > ));
    MPL_ASSERT_NOT(( is_sequence< int& > ));
    MPL_ASSERT_NOT(( is_sequence< UDT > ));
    MPL_ASSERT_NOT(( is_sequence< UDT* > ));
    MPL_ASSERT(( is_sequence< range_c<int,0,0> > ));
    MPL_ASSERT(( is_sequence< list<> > ));
    MPL_ASSERT(( is_sequence< list<int> > ));
    MPL_ASSERT(( is_sequence< vector<> > ));
    MPL_ASSERT(( is_sequence< vector<int> > ));
}


// polyvec
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/mc/raster_image_connectivity.hpp>
#include <polyvec/mc/get_bounding_box.hpp>
#include <polyvec/mc/bit_vector.hpp>
#include <polyvec/mc/associate_holes.hpp>

// libc++
#include <cstdio>
#include <memory>

// eigen
#include <Eigen/Geometry>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( mc )

constexpr int RasterImageConnectivity::infinite_region;

Eigen::Vector2i
RasterImageConnectivity::pixel_vertex_index_to_pos ( const int index ) {
    return Eigen::Vector2i ( index % pixel_vertex_dims().x(), index / pixel_vertex_dims().x() );
}

int
RasterImageConnectivity::pixel_vertex_pos_to_index ( const Eigen::Vector2i& pos ) {
    return pos.x() + pos.y() * pixel_vertex_dims().x();
}

int
RasterImageConnectivity::n_pixel_vertices() {
    return pixel_vertex_dims().x() * pixel_vertex_dims().y();
}

const Eigen::Vector2i&
RasterImageConnectivity::pixel_vertex_dims() const {
    return _pixel_vertex_dims;
}

Mesh_connectivity&
RasterImageConnectivity::connectivity() {
    return _connectivity;
}

int
RasterImageConnectivity::n_polygons() {
    return connectivity().n_active_faces();
}

int
RasterImageConnectivity::n_regions() {
    return _n_regions;
}

int
RasterImageConnectivity::n_region_polygons ( const int region_id ) {
    assert_break ( region_id < n_regions() );
    const int ans= _region_to_polygon_xval[region_id+1]-_region_to_polygon_xval[region_id];
    assert_break ( ans >= 1 );
    return ans;
}

int
RasterImageConnectivity::polygon_to_region ( const int polygon_id ) {
    assert_break ( polygon_id < n_polygons() );
    return _polygon_to_region[ polygon_id ];
}

void
RasterImageConnectivity::region_to_polygon ( const int region_id, int& outer_boundary, std::vector<int>& holes ) {
    outer_boundary = _region_to_polygon_val[ _region_to_polygon_xval[ region_id ] ];
    assert_break ( outer_boundary == region_id );

    holes.resize ( 0 );
    holes.reserve ( std::max ( 0, n_region_polygons ( region_id )-1 ) );

    for ( int j= _region_to_polygon_xval[ region_id ]+1 ; j < _region_to_polygon_xval[ region_id+1 ] ; ++j ) {
        holes.push_back (  _region_to_polygon_val [j] );
    }
}

RasterImageConnectivity::PolygonType
RasterImageConnectivity::get_polygon_type ( const int polygon_id ) {
    if ( polygon_id < n_regions() ) {
        return POLYGON_OUTER_BOUNDARY;
    } else if ( polygon_id < n_polygons() ) {
        return POLGYON_HOLE ;
    } else {
        assert_break ( 0 );
        return POLYGON_INVALID;
    }
}

Eigen::Matrix2Xi
RasterImageConnectivity::get_polygon_points ( const int pid ) {
    // Lambda to extract the points on polygon
    std::vector<int> points;

    Mesh_connectivity::Face_iterator face = connectivity().face_at ( pid );
    Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
    Mesh_connectivity::Half_edge_iterator he = he_end;

    int safe_guard = 0;
    const int safe_gaurd_cap = 10000;

    do {
        Eigen::Vector2i pos =  pixel_vertex_index_to_pos ( he.origin().index() );
        points.push_back ( pos.x() );
        points.push_back ( pos.y() );
        assert_break ( safe_guard < safe_gaurd_cap );
        ++safe_guard;
        he = he.next();
    } while ( !he.is_equal ( he_end ) );

    return Eigen::Matrix2Xi::ConstMapType ( points.data(), 2, points.size() /2 );
}

std::vector<int>
RasterImageConnectivity::regions_sorted_by_area() {
    std::vector<int> by_area(n_regions());
    std::vector<double> areas(n_regions());
    for (int i = 0; i < (int)by_area.size(); ++i) {
      by_area[i] = i;
      bool _1;
      Eigen::Matrix2Xi bdry = get_polygon_points(i);
      polyvec::WindingNumber::compute_orientation(bdry.cast<double>(), _1,
                                                  areas[i]);
    }
    std::sort(by_area.begin(), by_area.end(),
              [areas](int i, int j) { return areas[i] > areas[j]; });

    return by_area;
}

// dump as vtk for debugging
void
RasterImageConnectivity::dump_as_vtk ( const std::string filename ) {

    // Defrag the mesh
    Mesh_connectivity::Defragmentation_maps defrag;
    connectivity().compute_defragmention_maps ( defrag );

    FILE* fl = fopen ( filename.c_str(), "w" );
    assert_break ( fl && "FILE should be open" );

    /*
     * Write the vtk file.
     */

    // write the header
    fprintf ( fl, "# vtk DataFile Version 2.0\n" );
    fprintf ( fl, "Shayan's output mesh\n" );
    fprintf ( fl, "ASCII\n" );
    fprintf ( fl, "DATASET UNSTRUCTURED_GRID\n" );
    fprintf ( fl, "\n" );

    // write the vertices
    fprintf ( fl, "POINTS %d float\n", connectivity().n_active_vertices() );

    for ( int vnidx = 0; vnidx < connectivity().n_active_vertices(); vnidx++ ) {
        int voidx = defrag.new2old_vertices[vnidx];
        Eigen::Vector2i pos = pixel_vertex_index_to_pos ( voidx );
        fprintf ( fl, "%d %d 0 \n",  pos.x(), pos.y() );
    }

    fprintf ( fl, "\n" );

    //
    // write the faces
    //

    // count their total number of vertices.
    int total_vert_duplicated_per_face = 0;

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            ++total_vert_duplicated_per_face;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    }

    int face_verts_cache[4096]; // this must be more than maximum number of edges per face.
    fprintf ( fl, "CELLS %d %d \n", connectivity().n_active_faces(), connectivity().n_active_faces() + total_vert_duplicated_per_face );

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        int n_verts = 0;
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            face_verts_cache[n_verts] = defrag.old2new_vertices[he.origin().index()];
            ++n_verts;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );

        fprintf ( fl, "%d ", n_verts );

        for ( int voffset = 0; voffset < n_verts; ++voffset ) {
            //
            fprintf ( fl, "%d ", face_verts_cache[voffset] );
        }

        fprintf ( fl, "\n" );
    }

    fprintf ( fl, "\n" );

    // write the face types
    fprintf ( fl, "CELL_TYPES %d \n", connectivity().n_active_faces() );

    for ( int f = 0; f < connectivity().n_active_faces(); f++ ) {
        fprintf ( fl, "7 \n" ); // VTK POLYGON
    }

    fprintf ( fl, "\n" );

    // Now write the pixel indices
    fprintf ( fl, "POINT_DATA %d \n", connectivity().n_active_vertices() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "pixel_point_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_vertices(); i++ ) {
        int voidx = defrag.new2old_vertices[i];
        fprintf ( fl, "%d \n", voidx );
    }

// Write region colors?
    fprintf ( fl, "CELL_DATA %d \n", connectivity().n_active_faces() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "region_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_faces(); i++ ) {
        fprintf ( fl, "%d \n", i );
    }

    fclose ( fl );
}

void
RasterImageConnectivity::enumerate_holes ( Mesh_connectivity& pixconn, std::vector<int>& half_edge_on_hole ) {
    assert_break ( pixconn.n_active_half_edges() == pixconn.n_total_half_edges() );
    assert_break ( pixconn.n_active_faces() == pixconn.n_total_faces() );

    // Start marking all the half-edges
    Bit_vector is_half_edge_marked ( pixconn.n_active_half_edges() );

    // A lambda to traverse a hole and mark all the half edges on it
    auto traverse_bounday = [&] ( const int seed_he ) {
        Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
        Mesh_connectivity::Half_edge_iterator he = he_end;
        assert_break ( he.face().is_equal ( pixconn.hole() ) );

        int safe_guard = 0;
        const int safe_gaurd_cap = 10000;

        do {
            is_half_edge_marked.set ( he.index() );
            assert_break ( safe_guard < safe_gaurd_cap );
            ++safe_guard;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    };

    // Loop over all half-edges, if not marked and boundary, it is a hole
    half_edge_on_hole.resize ( 0 );

    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( heid );

        if ( he.face().is_equal ( pixconn.hole() ) && ( !is_half_edge_marked[heid] ) ) {
            half_edge_on_hole.push_back ( heid );
            traverse_bounday ( heid );
        }
    }
}

// constructor
RasterImageConnectivity
RasterImageConnectivity::build ( const std::vector<Eigen::Matrix2Xi>& region_boundaries ) {

    RasterImageConnectivity ans;

    //
    // First find the bounding box of the image
    //
    {
        Eigen::AlignedBox<int, 2> bbox = get_bounding_box ( region_boundaries );
        // Make sure min is 0,0. This must have been done in get_pixel_regions()
        assert_break ( bbox.min() == Eigen::Vector2i ( 0, 0 ) );
        // Set the dims to the max
        ans._pixel_vertex_dims = bbox.max()+Eigen::Vector2i ( 1, 1 );
    }

    //
    // Now bould the connectivity by treating each region as a BIG polygon
    //
    {
        std::vector<int> polygon_verts;
        std::vector<int> polygon_xadj = {0};

        for ( int i = 0 ; i < ( int ) region_boundaries.size() ; ++i ) {
            polygon_xadj.push_back ( polygon_xadj.back() );
            const Eigen::Matrix2Xi& region_boundary = region_boundaries[i];

            for ( int j = 0 ; j < ( int ) region_boundary.cols() ; ++j ) {
                polygon_verts.push_back ( ans.pixel_vertex_pos_to_index ( region_boundary.col ( j ) ) );
                ++polygon_xadj.back();
            }
        }

        ans._connectivity.build_from_polygons ( ans.n_pixel_vertices(), polygon_verts, polygon_xadj );
    }

    //
    // Now get all the holes and add them
    // Even the outer boundary -- it should be fine
    //
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // Set number of regions
        ans._n_regions = ( int ) region_boundaries.size();

        // A lambda to modify the connectivity
        auto associate_face_and_he = [&] ( const int seed_he, const int parent_face ) {
            Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
            Mesh_connectivity::Half_edge_iterator he = he_end;
            Mesh_connectivity::Face_iterator face = pixconn.face_at ( parent_face );
            assert_break ( he.face().is_equal ( pixconn.hole() ) );

            int safe_guard = 0;
            const int safe_gaurd_cap = 10000;

            // Make the association
            face.data().half_edge = he.index();

            do {
                he.data().face = face.index();
                assert_break ( safe_guard < safe_gaurd_cap );
                ++safe_guard;
                he = he.next();
            } while ( !he.is_equal ( he_end ) );
        };

        // Get all the holes
        std::vector<int> he_on_holes;
        enumerate_holes ( pixconn,  he_on_holes );

        for ( int heid: he_on_holes ) {
            Mesh_connectivity::Face_iterator face =  pixconn.add_face();
            associate_face_and_he ( heid, face.index() );
        }

    } // End of creating the hole polygons

    // Now associate holes and boundary (polygons to regions)
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // No extract the points
        // Get the holes and polygons as CCW paths
        // And also find the areas
        std::vector<Eigen::Matrix2Xd> outer_boundaries;
        std::vector<double> outer_boundary_areas;
        std::vector<Eigen::Matrix2Xd> holes;
        std::vector<double> hole_areas;

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            outer_boundaries.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( outer_boundaries.back(), is_ccw, area );
            assert_break ( is_ccw );
            outer_boundary_areas.push_back ( area );
        }

        for ( int i = ans.n_regions()  ; i < ans.n_polygons() ; ++i ) {
            holes.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            holes.back().rowwise().reverseInPlace(); // make CCW
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( holes.back(), is_ccw, area );
            assert_break ( is_ccw );
            hole_areas.push_back ( area );
        }

        //
        // Create lambdas that say if a polygon is inside another
        //  assumes that out and in are both pixel boundaries
        // and are ccw
        // Only works if the area of out is bigger than in
        //
        auto is_inside = [&] ( const Eigen::Matrix2Xd &out, const Eigen::Matrix2Xd &in ) {
            const Eigen::Vector2d mid = (in.col ( 0 ) + in.col ( 1 )) /2.;
            const Eigen::Vector2d tang = in.col ( 1 ) -  in.col ( 0 );
            const Eigen::Vector2d normal ( -tang.y(), tang.x() );
            const Eigen::Vector2d definitely_inside_in = normal/2. + mid;

            bool is_trustable;
            double winding;
            ::polyvec::WindingNumber::compute_winding ( out, definitely_inside_in, winding, is_trustable );
            assert_break ( is_trustable );
            return std::abs ( winding ) > 1e-5;
        };

        // Now call the association function
        std::vector<std::vector<int>> boundary_holes;
        associate_holes (
            ( int ) outer_boundaries.size(),
            ( int ) holes.size(),
        [&] ( const int bout, const int hin ) {
            if ( ( int ) outer_boundary_areas[bout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( outer_boundaries[bout], holes[hin] );
            }
        },
        [&] ( const int hout, const int hin ) {
            if ( ( int ) hole_areas[hout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( holes[hout], holes[hin] );
            }
        },
        boundary_holes );

        // Now build the adj and xadj
        ans._polygon_to_region.resize ( ans.n_polygons(), infinite_region );
        ans._region_to_polygon_xval.resize ( 1, 0 );
        ans._region_to_polygon_val.resize ( 0 );

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            const int region_id = i;
            ans._region_to_polygon_xval.push_back ( ans._region_to_polygon_xval.back() );

            // Don't forget yourself :)
            ans._polygon_to_region[region_id] = region_id;
            ++ans._region_to_polygon_xval.back();
            ans._region_to_polygon_val.push_back ( region_id );

            for ( int  j = 0 ; j < ( int ) boundary_holes[region_id].size() ; ++j ) {
                const int polygon_id = ans.n_regions() + boundary_holes[region_id][j];
                ans._polygon_to_region[polygon_id] = region_id;
                ++ans._region_to_polygon_xval.back();
                ans._region_to_polygon_val.push_back ( polygon_id );
            }
        }
    } // ALL DONE WITH REGION HOLE ASSOCIATION

    return ans;
}

NAMESPACE_END ( mc )
NAMESPACE_END ( polyfit )


// The MIT License
// (c) 2019 Daniel Williams

#ifndef _GRID_GRID_HPP_
#define _GRID_GRID_HPP_

#include <cmath>
#include <memory>
#include <cstdlib>
#include <string>
#include <vector>

#include <Eigen/Geometry>

#include "nlohmann/json/json.hpp"

#include "geometry/cell_value.hpp"
#include "geometry/polygon.hpp"
#include "geometry/layout.hpp"

using terrain::geometry::Layout;
using terrain::geometry::cell_value_t;
using terrain::geometry::Polygon;

namespace terrain::grid {

class Grid {
public:
    /**
     * Constructs a new quad tree, centered at 0,0 and 1024 units wide, square
     *
     * Use this constructor if the tree will be loaded from a config file, and the initialization values do not matter.
     */
    Grid();

    /**
     * Constructs a new 2d square grid
     *
     * @param {_dim} number of cells along each dimensions of this grid
     * @param {_bounds} bounds which this grid will represent
     */
    Grid(const Layout& _layout);

    /**
     *  Releases all memory associated with this quad tree.
     */
    ~Grid(){};

    /**
     * Returns true if the point at (x, y) exists in the tree.
     *
     * @param The p to test
     * @return Whether the tree contains the given point
     */
    bool contains(const Eigen::Vector2d& p) const;

    ///! \brief Draws a simple debug representation of this grid to stderr
    void debug() const;

    ///! \brief sets the entire grid to the given value
    ///! \param fill_value - fill value for entire grid
    void fill(const cell_value_t fill_value);

    ///! \brief Fills the _interior_ of the given polygon with the given value.
    ///!
    ///! @param source - polygon defining the fill araea. Assumed to be closed, CCW, and non-intersecting
    ///! @param fill_value -fill value for area
    void fill(const Polygon& source, const cell_value_t fill_value);

    /**
     * Get the overall bounds of this tree
     *
     * @return Bounds object describing the tree's overall bounds.
     */
    inline const Layout& get_layout() const { return layout; }

    ///! \brief simply returns the value or reference to the internal data
    ///! \warning !! DOES NOT CHECK BOUNDS !!
    cell_value_t& get_cell(const size_t xi, const size_t yi);
    cell_value_t get_cell(const size_t xi, const size_t yi) const ;

    constexpr double get_load_factor() const { return 1.0; }

    size_t get_memory_usage() const;

    ///! the spacing of each cell === center-to-center distance. === cell-width.
    double get_precision() const;

    inline void prune() {};

    void reset();
    void reset(const Layout& _layout);

    ///! \brief Retrieve the value at an (x, y) Eigen::Vector2d
    ///!
    ///! \param Eigen::Vector2d - the x,y coordinates to search at
    ///! \return the cell value
    cell_value_t classify(const Eigen::Vector2d& p) const;

    ///! \brief the _total_ number of cells in this grid === (width * height)
    size_t size() const;

    ///! \brief Access the value at an (x, y) Eigen::Vector2d
    ///!
    ///! \param Eigen::Vector2d - the x,y coordinates to search at:
    ///! \return reference to the cell value
    bool store(const Eigen::Vector2d& p, const cell_value_t new_value);

    bool to_png(const std::string filename) const;

public:
    ///! the data layout this grid represents
    Layout layout;

    // raw array:  2D addressing is performed through the class methods
    std::vector<cell_value_t> storage;

private:
    friend class GridTest_SnapPrecision_Test;
    friend class GridTest_XYToIndex_Test;

};

}; // namespace terrain::grid

#endif // _GRID_HPP_


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE extracttuples_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(extracttuples_test_simple)
{
    IndexArrayType rows    = {0, 0, 0, 1, 1, 1, 2, 2};
    IndexArrayType columns = {1, 2, 3, 0, 2, 3, 0, 1};
    std::vector<double> values        = {1, 2, 3, 4, 6, 7, 8, 9};
    Matrix<double, DirectedMatrixTag> m1(3, 4);
    m1.build(rows, columns, values);

    IndexType nnz = m1.nvals();
    IndexArrayType r(nnz), c(nnz);
    std::vector<double> v(nnz);

    m1.extractTuples(r, c, v);

    /// @todo the following check should be a utility function

    // Check the result, but it may be out of order.
    bool success = true;
    for (IndexType ix = 0; ix < values.size(); ++ix)
    {
        // Note: no semantics defined for extractTuples regarding the
        // order of returned values, so using an O(N^2) approach
        // without sorting:
        bool found = false;
        for (IndexType iy = 0; iy < v.size(); ++iy)
        {
            if ((r[iy] == rows[ix]) && (c[iy] == columns[ix]))
            {
                found = true;
                if (v[iy] != values[ix])
                {
                    success = false;
                }
                break;
            }
        }
        if (!found)
        {
            success = false;
        }
    }

    BOOST_CHECK_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END()


#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/MSHFieldParser.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <MeshFEM/util.h>
#include <vector>
#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>

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

namespace po = boost::program_options;
using namespace std;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: Simulate_cli [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m",           po::value<string>()->default_value(""), "simulation material material")
        ("matFieldName,f",       po::value<string>()->default_value(""), "name of material field to load from .msh passed as --material")
        ("boundaryConditions,b", po::value<string>(),                    "boundary conditions")
        ("outputMSH,o",          po::value<string>(),                    "output mesh")
        ("dumpMatrix",           po::value<string>()->default_value(""), "dump system matrix in triplet format")
        ("degree,d",             po::value<int>()->default_value(2),     "FEM degree (1 or 2)")
        ("fullDegreeFieldOutput,D",                                      "Output full-degree nodal fields (don't do piecewise linear subsample)")
        ("extraMesh,e",          po::value<string>(),                    "adds another independent input mesh to problem")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    if (vm["dumpMatrix"].as<string>().size() == 0) {
        if (vm.count("outputMSH") == 0) {
            cout << "Error: must specify output msh file (unless dumping a stiffness matrix)" << endl;
            fail = true;
        }
    }

    if (vm.count("outputMSH") && (vm.count("boundaryConditions") == 0)) {
        cout << "Error: must specify boundary conditions to run a simulation" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

template<size_t _N, size_t _Deg>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    size_t numElements = inElements.size();
    typedef LinearElasticity::Mesh<_N, _Deg> Mesh;
    using Simulator = LinearElasticity::Simulator<Mesh>;
    Simulator sim(inElements, inVertices);

    typedef ScalarField<Real> SField;
    const string &materialPath = args[    "material"].as<string>(),
                 &matFieldName = args["matFieldName"].as<string>(),
                 &matrixPath   = args[  "dumpMatrix"].as<string>();

    string bcPath, outMSH;
    if (args.count("boundaryConditions")) bcPath = args["boundaryConditions"].as<string>();
    if (args.count(         "outputMSH")) outMSH = args[         "outputMSH"].as<string>();

    if (fileExtension(materialPath) == ".msh") {
        MSHFieldParser<_N> fieldParser(materialPath);
        // Read heterogenous material from .msh file.
        // Guess isotropic or orhotropic based on fields present
        // Isotropic names: E nu
        // Orthotropic names: E_x E_y [E_z] nu_yx [nu_zx nu_zy] [mu_yz mu_zx] mu[_xy]
        auto domainSizeChecker = [=](const vector<SField> &fs) -> bool {
            return all_of(fs.begin(), fs.end(),
               [=](const SField &f) { return f.domainSize() == numElements; } ); };
        runtime_error sizeErr("Material parameter fields of incorrect size.");
        runtime_error notFound("No complete material parameter field was found.");

        vector<SField> paramFields;
        vector<string> isotropicNames = { "E", "nu" };
        for (string name : isotropicNames) {
            name = matFieldName + name;
            try { paramFields.push_back(fieldParser.scalarField(name,
                        DomainType::PER_ELEMENT)); }
            catch (...) { /* Don't complain yet--try orthotropic */ }
        }
        if (paramFields.size() == 2) {
            if (!domainSizeChecker(paramFields)) throw sizeErr;
            // Valid isotropic material field--load it into simulator.
            LinearElasticity::ETensorStoreGetter<_N> store;
            for (size_t i = 0; i < sim.mesh().numElements(); ++i) {
                store().setIsotropic(paramFields[0][i], paramFields[1][i]);
                sim.mesh().element(i)->configure(store);
            }
            cout << "Loaded " << _N << "D isotropic material" << endl;
        }
        else {
            // If isotropic field wasn't found, try orthotropic.
            paramFields.clear();
            vector<vector<string> > orthotropicNames =
                { { "E_x", "E_y", "nu_yx", "mu" },
                  { "E_x", "E_y", "E_z", "nu_yx", "nu_zx", "nu_zy", "mu_yz", "mu_zx", "mu_xy" } };
            for (string name : orthotropicNames.at(_N - 2)) {
                name = matFieldName + name;
                try { paramFields.push_back(fieldParser.scalarField(name,
                            DomainType::PER_ELEMENT)); }
                catch (...) { throw notFound; }
            }
            if (!domainSizeChecker(paramFields)) throw sizeErr;
            // Valid orthotropic material field--load it into simulator.
            LinearElasticity::ETensorStoreGetter<_N> store;
            for (size_t i = 0; i < sim.mesh().numElements(); ++i) {
                if (_N == 2) {
                    store().setOrthotropic2D(
                        paramFields[0][i], paramFields[1][i],
                        paramFields[2][i], paramFields[3][i]);
                }
                else {
                    store().setOrthotropic3D(
                        paramFields[0][i], paramFields[1][i], paramFields[2][i],
                        paramFields[3][i], paramFields[4][i], paramFields[5][i],
                        paramFields[6][i], paramFields[7][i], paramFields[8][i]);
                }
                sim.mesh().element(i)->configure(store);
            }
            cout << "Loaded " << _N << "D Orthotropic material" << endl;
        }
    }
    else {
        // Read homogenous material from .material file (or use default material
        // if no file is given).
        Materials::Constant<_N> mat;
        if (materialPath != "")
            mat.setFromFile(materialPath);
        LinearElasticity::ETensorStoreGetter<_N> store(mat.getTensor());
        for (size_t i = 0; i < sim.mesh().numElements(); ++i)
            sim.mesh().element(i)->configure(store);
    }

    // Check if we're just dumping the stiffness matrix without simulating
    if ((matrixPath != "") && (bcPath == "")) {
        typename Simulator::TMatrix K;
        sim.m_assembleStiffnessMatrix(K);
        K.sumRepeated();
        K.dumpBinary(matrixPath);
        exit(0);
    }

    bool noRigidMotion;
    vector<PeriodicPairDirichletCondition<_N>> pps;
    ComponentMask pinTranslationComponents;
    auto bconds = readBoundaryConditions<_N>(bcPath, sim.mesh().boundingBox(), noRigidMotion, pps, pinTranslationComponents);
    sim.applyTranslationPins(pinTranslationComponents);
    sim.applyBoundaryConditions(bconds);
    sim.applyPeriodicPairDirichletConditions(pps);
    if (noRigidMotion) sim.applyNoRigidMotionConstraint();

    if (matrixPath != "") sim.dumpSystem(matrixPath);

    BENCHMARK_START_TIMER_SECTION("Simulation");
    auto u = sim.solve();
    auto e = sim.averageStrainField(u);
    auto s = sim.averageStressField(u);
    auto f = sim.dofToNodeField(sim.neumannLoad());
    BENCHMARK_STOP_TIMER_SECTION("Simulation");

    bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;

    MSHFieldWriter writer(outMSH, sim.mesh(), linearSubsampleFields);
    writer.addField("u",      u, DomainType::PER_NODE);
    writer.addField("load",   f, DomainType::PER_NODE);
    if ((Simulator::Strain::Deg == 0) || linearSubsampleFields) {
        // Output constant (average) strain/stress for piecewise linear u
        writer.addField("strain", e, DomainType::PER_ELEMENT);
        writer.addField("stress", s, DomainType::PER_ELEMENT);
    }
    else {
        // Output full-degree per-element strain. (Wasteful since
        // strain fields are of degree - 1, but Gmsh/MSHFieldWriter
        // only supports full-degree ElementNodeData).
        auto linearField = sim.strainField(u);
        using Upsampled = SymmetricMatrixInterpolant<typename Simulator::SMatrix, _N, _Deg>;
        vector<Upsampled> upsampledField;
        upsampledField.reserve(linearField.size());
        for (const auto ss: linearField) upsampledField.emplace_back(ss);
        writer.addField("strain", upsampledField, DomainType::PER_ELEMENT);

        linearField = sim.stressField(u);
        upsampledField.clear();
        for (const auto ss: linearField) upsampledField.emplace_back(ss);
        writer.addField("stress", upsampledField, DomainType::PER_ELEMENT);
    }

    // // Write mat parameter fields
    // SField Ex(numElements), Ey(numElements), nuYX(numElements), mu(numElements);
    // for (size_t i = 0; i < sim.mesh().numElements(); ++i)
    //     sim.mesh().element(i)->E().getOrthotropic2D(Ex[i], Ey[i], nuYX[i], mu[i]);
    // writer.addField("E_x",    Ex,    DomainType::PER_ELEMENT);
    // writer.addField("E_y",    Ey,    DomainType::PER_ELEMENT);
    // writer.addField("nu_yx",  nuYX,  DomainType::PER_ELEMENT);
    // writer.addField("mu",     mu,    DomainType::PER_ELEMENT);

    sim.reportRegionSurfaceForces(u);
    writer.addField("Ku", sim.applyStiffnessMatrix(u), DomainType::PER_NODE);

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();

    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS, MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    size_t dim;
    if      (type == MeshIO::MESH_TET) dim = 3;
    else if (type == MeshIO::MESH_TRI) dim = 2;
    else    throw std::runtime_error("Mesh must be pure triangle or tet.");

    if (args.count("extraMesh") > 0) {
        // loads second mesh
        vector<MeshIO::IOVertex>  inExtraVertices;
        vector<MeshIO::IOElement> inExtraElements;
        string extraMeshPath = args["extraMesh"].as<string>();
        auto typeExtra = load(extraMeshPath, inExtraVertices, inExtraElements, MeshIO::FMT_GUESS, MeshIO::MESH_GUESS);

        if (type != typeExtra) {
            std::cerr << "Extra mesh of different type." << std::endl;
            throw std::runtime_error("Extra mesh of different type.");
        }

        //std::cout << "Original Vertices: " <<  inVertices.size() << std::endl;
        //std::cout << "Original Elements: " <<  inElements.size() << std::endl << std::endl;

        //save("misc/experiments/multiple_meshes/original.off", inVertices, inElements);
        //save("misc/experiments/multiple_meshes/original.msh", inVertices, inElements);

        //std::cout << "Extra Vertices: " <<  inExtraVertices.size() << std::endl;
        //std::cout << "Extra Elements: " <<  inExtraElements.size() << std::endl << std::endl;

        //save("misc/experiments/multiple_meshes/extra.off", inExtraVertices, inExtraElements);
        //save("misc/experiments/multiple_meshes/extra.msh", inExtraVertices, inExtraElements);

        // Adjust vertices indices for extra elements
        for (size_t e=0; e<inExtraElements.size(); e++) {
            for (size_t i=0; i<(dim+1); i++) {
                inExtraElements[e][i] = inExtraElements[e][i] + inVertices.size();
            }
        }

        // Add vertices and elements of second mesh to lists passed to simulator
        inVertices.insert(inVertices.end(), inExtraVertices.begin(), inExtraVertices.end());
        inElements.insert(inElements.end(), inExtraElements.begin(), inExtraElements.end());

        //std::cout << "Total Vertices: " <<  inVertices.size() << std::endl;
        //std::cout << "Total Elements: " <<  inElements.size() << std::endl << std::endl;

        //save("misc/experiments/multiple_meshes/together.off", inVertices, inElements);
        //save("misc/experiments/multiple_meshes/together.msh", inVertices, inElements);
    }

    // Look up and run appropriate simulation instantiation.
    int deg = args["degree"].as<int>();
    auto exec = (dim == 3) ? ((deg == 2) ? execute<3, 2> : execute<3, 1>)
                           : ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015-2017.
// Modifications copyright (c) 2015-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP

#include <cstddef>
#include <utility>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/math.hpp>

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

#include <boost/geometry/formulas/vertex_latitude.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>

#include <boost/geometry/algorithms/detail/envelope/point.hpp>
#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>

#include <boost/geometry/algorithms/detail/expand/point.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

template <typename CalculationType, typename CS_Tag>
struct envelope_segment_call_vertex_latitude
{
    template <typename T1, typename T2, typename Strategy>
    static inline CalculationType apply(T1 const& lat1,
                                        T2 const& alp1,
                                        Strategy const& )
    {
        return geometry::formula::vertex_latitude<CalculationType, CS_Tag>
            ::apply(lat1, alp1);
    }
};

template <typename CalculationType>
struct envelope_segment_call_vertex_latitude<CalculationType, geographic_tag>
{
    template <typename T1, typename T2, typename Strategy>
    static inline CalculationType apply(T1 const& lat1,
                                        T2 const& alp1,
                                        Strategy const& strategy)
    {
        return geometry::formula::vertex_latitude<CalculationType, geographic_tag>
            ::apply(lat1, alp1, strategy.model());
    }
};

template <typename CS_Tag>
class envelope_segment_impl
{
private:

    // degrees or radians
    template <typename CalculationType>
    static inline void swap(CalculationType& lon1,
                            CalculationType& lat1,
                            CalculationType& lon2,
                            CalculationType& lat2)
    {
        std::swap(lon1, lon2);
        std::swap(lat1, lat2);
    }

    // radians
    template <typename CalculationType>
    static inline bool contains_pi_half(CalculationType const& a1,
                                        CalculationType const& a2)
    {
        // azimuths a1 and a2 are assumed to be in radians
        BOOST_GEOMETRY_ASSERT(! math::equals(a1, a2));

        static CalculationType const pi_half = math::half_pi<CalculationType>();

        return (a1 < a2)
                ? (a1 < pi_half && pi_half < a2)
                : (a1 > pi_half && pi_half > a2);
    }

    // radians or degrees
    template <typename Units, typename CoordinateType>
    static inline bool crosses_antimeridian(CoordinateType const& lon1,
                                            CoordinateType const& lon2)
    {
        typedef math::detail::constants_on_spheroid
            <
                CoordinateType, Units
            > constants;

        return math::abs(lon1 - lon2) > constants::half_period(); // > pi
    }

    // degrees or radians
    template <typename Units, typename CalculationType, typename Strategy>
    static inline void compute_box_corners(CalculationType& lon1,
                                           CalculationType& lat1,
                                           CalculationType& lon2,
                                           CalculationType& lat2,
                                           Strategy const& strategy)
    {
        // coordinates are assumed to be in radians
        BOOST_GEOMETRY_ASSERT(lon1 <= lon2);

        CalculationType lon1_rad = math::as_radian<Units>(lon1);
        CalculationType lat1_rad = math::as_radian<Units>(lat1);
        CalculationType lon2_rad = math::as_radian<Units>(lon2);
        CalculationType lat2_rad = math::as_radian<Units>(lat2);

        CalculationType a1, a2;
        strategy.apply(lon1_rad, lat1_rad, lon2_rad, lat2_rad, a1, a2);

        if (lat1 > lat2)
        {
            std::swap(lat1, lat2);
            std::swap(lat1_rad, lat2_rad);
            std::swap(a1, a2);
        }

        if (math::equals(a1, a2))
        {
            // the segment must lie on the equator or is very short
            return;
        }

        if (contains_pi_half(a1, a2))
        {
            CalculationType p_max = envelope_segment_call_vertex_latitude
                <CalculationType, CS_Tag>::apply(lat1_rad, a1, strategy);

            CalculationType const mid_lat = lat1 + lat2;
            if (mid_lat < 0)
            {
                // update using min latitude
                CalculationType const lat_min_rad = -p_max;
                CalculationType const lat_min
                    = math::from_radian<Units>(lat_min_rad);

                if (lat1 > lat_min)
                {
                    lat1 = lat_min;
                }
            }
            else if (mid_lat > 0)
            {
                // update using max latitude
                CalculationType const lat_max_rad = p_max;
                CalculationType const lat_max
                    = math::from_radian<Units>(lat_max_rad);

                if (lat2 < lat_max)
                {
                    lat2 = lat_max;
                }
            }
        }
    }

    template <typename Units, typename CalculationType, typename Strategy>
    static inline void apply(CalculationType& lon1,
                             CalculationType& lat1,
                             CalculationType& lon2,
                             CalculationType& lat2,
                             Strategy const& strategy)
    {
        typedef math::detail::constants_on_spheroid
            <
                CalculationType, Units
            > constants;

        bool is_pole1 = math::equals(math::abs(lat1), constants::max_latitude());
        bool is_pole2 = math::equals(math::abs(lat2), constants::max_latitude());

        if (is_pole1 && is_pole2)
        {
            // both points are poles; nothing more to do:
            // longitudes are already normalized to 0
            // but just in case
            lon1 = 0;
            lon2 = 0;
        }
        else if (is_pole1 && !is_pole2)
        {
            // first point is a pole, second point is not:
            // make the longitude of the first point the same as that
            // of the second point
            lon1 = lon2;
        }
        else if (!is_pole1 && is_pole2)
        {
            // second point is a pole, first point is not:
            // make the longitude of the second point the same as that
            // of the first point
            lon2 = lon1;
        }

        if (lon1 == lon2)
        {
            // segment lies on a meridian
            if (lat1 > lat2)
            {
                std::swap(lat1, lat2);
            }
            return;
        }

        BOOST_GEOMETRY_ASSERT(!is_pole1 && !is_pole2);

        if (lon1 > lon2)
        {
            swap(lon1, lat1, lon2, lat2);
        }

        if (crosses_antimeridian<Units>(lon1, lon2))
        {
            lon1 += constants::period();
            swap(lon1, lat1, lon2, lat2);
        }

        compute_box_corners<Units>(lon1, lat1, lon2, lat2, strategy);
    }

public:
    template <
            typename Units,
            typename CalculationType,
            typename Box,
            typename Strategy
            >
    static inline void apply(CalculationType lon1,
                             CalculationType lat1,
                             CalculationType lon2,
                             CalculationType lat2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        typedef typename coordinate_type<Box>::type box_coordinate_type;

        typedef typename helper_geometry
            <
                Box, box_coordinate_type, Units
            >::type helper_box_type;

        helper_box_type radian_mbr;

        apply<Units>(lon1, lat1, lon2, lat2, strategy);

        geometry::set
            <
                min_corner, 0
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lon1));

        geometry::set
            <
                min_corner, 1
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lat1));

        geometry::set
            <
                max_corner, 0
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lon2));

        geometry::set
            <
                max_corner, 1
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lat2));

        transform_units(radian_mbr, mbr);
    }
};

template <std::size_t Dimension, std::size_t DimensionCount>
struct envelope_one_segment
{
    template<typename Point, typename Box, typename Strategy>
    static inline void apply(Point const& p1,
                             Point const& p2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        envelope_one_point<Dimension, DimensionCount>::apply(p1, mbr, strategy);
        detail::expand::point_loop
            <
                strategy::compare::default_strategy,
                strategy::compare::default_strategy,
                Dimension,
                DimensionCount
            >::apply(mbr, p2, strategy);
    }
};

template <std::size_t DimensionCount>
struct envelope_segment
{
    template <typename Point, typename Box, typename Strategy>
    static inline void apply(Point const& p1,
                             Point const& p2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        // first compute the envelope range for the first two coordinates
        strategy.apply(p1, p2, mbr);

        // now compute the envelope range for coordinates of
        // dimension 2 and higher
        envelope_one_segment<2, DimensionCount>::apply(p1, p2, mbr, strategy);
    }

    template <typename Segment, typename Box>
    static inline void apply(Segment const& segment, Box& mbr)
    {
        typename point_type<Segment>::type p[2];
        detail::assign_point_from_index<0>(segment, p[0]);
        detail::assign_point_from_index<1>(segment, p[1]);
        apply(p[0], p[1], mbr);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Segment>
struct envelope<Segment, segment_tag>
{
    template <typename Box, typename Strategy>
    static inline void apply(Segment const& segment,
                             Box& mbr,
                             Strategy const& strategy)
    {
        typename point_type<Segment>::type p[2];
        detail::assign_point_from_index<0>(segment, p[0]);
        detail::assign_point_from_index<1>(segment, p[1]);
        detail::envelope::envelope_segment
            <
               dimension<Segment>::value
            >::apply(p[0], p[1], mbr, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP