#ifndef FIBER_HPP
#define FIBER_HPP

#include <skelly_sim.hpp>

#include <Eigen/LU>
#include <list>
#include <unordered_map>

#include <kernels.hpp>
#include <params.hpp>

class Periphery;

/// @brief Class to represent a single flexible filament
///
/// Actions on the fiber class are typically handled via the container object, which will
/// distribute calls appropriately across all fibers in the container.
class Fiber {
  public:
    enum BC { Force, Torque, Velocity, AngularVelocity, Position, Angle };
    static const std::string BC_name[];

    // Input parameters
    int n_nodes_;                  ///< number of nodes representing the fiber
    double length_;                ///< Desired 'constraint' length of fiber
    double length_prev_;           ///< Last accepted length_
    double bending_rigidity_;      ///< bending rigidity 'E' of fiber
    double penalty_param_ = 500.0; ///< @brief Tension penalty parameter for linear operator @see update_linear_operator
    /// @brief scale of external force on node @see generate_external_force
    /// \f[{\bf f} = f_s * {\bf x}_s\f]
    double force_scale_ = 0.0;
    // FIXME: Magic numbers in linear operator calculation
    double beta_tstep_ = 1.0; ///< penalty parameter to ensure inextensibility
    double epsilon_ = 1E-3;   ///< slenderness parameter

    /// (body, site) pair for minus end binding. -1 implies unbound
    std::pair<int, int> binding_site_{-1, -1};

    double v_growth_ = 0.0;      ///< instantaneous fiber growth velocity
    bool near_periphery = false; ///< flag if interacting with periphery

    /// @brief Coefficient for SBT @see Fiber::init
    /// \f[ c_0 = -\frac{log(e \epsilon^\ell)}{8 \pi \eta}\f]
    double c_0_;

    /// @brief Coefficient for SBT @see Fiber::init
    /// \f[ c_1 = \frac{1}{4\pi\eta} \f]
    double c_1_;

    /// Boundary condition pair for minus end of fiber
    std::pair<BC, BC> bc_minus_ = {BC::Velocity, BC::AngularVelocity};
    /// Boundary condition pair for plus end of fiber
    std::pair<BC, BC> bc_plus_ = {BC::Force, BC::Torque};

    Eigen::MatrixXd x_;     ///< [ 3 x n_nodes_ ] matrix representing coordinates of fiber nodes
    Eigen::MatrixXd xs_;    ///< [ 3 x n_nodes_ ] matrix representing first derivative of fiber nodes
    Eigen::MatrixXd xss_;   ///< [ 3 x n_nodes_ ] matrix representing second derivative of fiber nodes
    Eigen::MatrixXd xsss_;  ///< [ 3 x n_nodes_ ] matrix representing third derivative of fiber nodes
    Eigen::MatrixXd xssss_; ///< [ 3 x n_nodes_ ] matrix representing fourth derivative of fiber nodes

    /// [ 3*n_nodes_ x 3*n_nodes_] Oseen tensor for fiber @see Fiber::update_stokeslet
    Eigen::MatrixXd stokeslet_;

    Eigen::MatrixXd A_;                         ///< Fiber's linear operator for matrix solver
    Eigen::FullPivLU<Eigen::MatrixXd> A_LU_; ///< Fiber preconditioner, LU decomposition of Fiber::A_
    /// Fiber force operator, @see Fiber::update_force_operator, FiberContainer::apply_fiber_force
    Eigen::MatrixXd force_operator_;
    Eigen::VectorXd RHS_; ///< Current 'right-hand-side' for matrix formulation of solver

    /// Structure that caches arrays useful for calculating various fiber values
    typedef struct {
        Eigen::ArrayXd alpha;
        Eigen::ArrayXd alpha_roots;
        Eigen::ArrayXd alpha_tension;
        Eigen::ArrayXd weights_0;
        Eigen::MatrixXd D_1_0;
        Eigen::MatrixXd D_2_0;
        Eigen::MatrixXd D_3_0;
        Eigen::MatrixXd D_4_0;
        Eigen::MatrixXd P_X;
        Eigen::MatrixXd P_T;
        Eigen::MatrixXd P_downsample_bc;
    } fib_mat_t;

    /// Map of cached matrices for different values of n_nodes_. Calculated automagically at program start. @see
    /// compute_matrices
    const static std::unordered_map<int, fib_mat_t> matrices_;

    Fiber(toml::value &fiber_table, double eta);
    Fiber() = default;

    /// @brief initialize empty fiber
    /// @param[in] n_nodes fiber 'resolution'
    /// @param[in] bending_rigidity bending rigidity of fiber
    /// @param[in] eta fluid viscosity
    ///
    /// @deprecated Initializing with a toml::table structure is the preferred initialization. This is only around for
    /// testing.
    Fiber(int n_nodes, double bending_rigidity, double eta) : n_nodes_(n_nodes), bending_rigidity_(bending_rigidity) {
        init(eta);
    };

    ///< @brief Set some default values and resize arrays
    ///
    ///< _MUST_ be called from constructors.
    ///
    /// Initializes: Fiber::x_, Fiber::xs_, Fiber::xss_, Fiber::xsss_, Fiber::xssss_, Fiber::c_0_, Fiber::c_1_
    void init(double eta) {
        x_ = Eigen::MatrixXd::Zero(3, n_nodes_);
        x_.row(0) = Eigen::ArrayXd::LinSpaced(n_nodes_, 0, 1.0).transpose();
        xs_.resize(3, n_nodes_);
        xss_.resize(3, n_nodes_);
        xsss_.resize(3, n_nodes_);
        xssss_.resize(3, n_nodes_);
        length_prev_ = length_;

        c_0_ = -log(M_E * std::pow(epsilon_, 2)) / (8 * M_PI * eta);
        c_1_ = 2.0 / (8.0 * M_PI * eta);
    };

    void update_preconditioner();
    void update_force_operator();
    void update_RHS(double dt, MatrixRef &flow, MatrixRef &f_external);
    void update_linear_operator(double dt, double eta);
    void apply_bc_rectangular(double dt, MatrixRef &v_on_fiber, MatrixRef &f_on_fiber);
    void translate(const Eigen::Vector3d &r) { x_.colwise() += r; };
    void update_derivatives();
    void update_stokeslet(double);
    bool attached_to_body() { return binding_site_.first >= 0; };
    MSGPACK_DEFINE_MAP(n_nodes_, length_, bending_rigidity_, penalty_param_, force_scale_, beta_tstep_, epsilon_,
                       binding_site_, x_);
};

/// Class to hold the fiber objects.
///
/// The container object is designed to work on fibers local to that MPI rank. Each MPI rank
/// should have its own container, with its own unique fibers. The container object does not
/// have any knowledge of the MPI world state, which, for example, is passed in externally to
/// the FiberContainer::flow method and potentially others.
///
/// Developer note: ideally all interactions with the fiber objects should be through this
/// container, except for testing purposes. Operating on fibers outside of the container class
/// is ill-advised.
class FiberContainer {
  public:
    std::list<Fiber> fibers; ///< Array of fibers local to this MPI rank
    /// pointer to FMM object (pointer to avoid constructing stokeslet_kernel_ with default FiberContainer)
    std::shared_ptr<kernels::FMM<stkfmm::Stk3DFMM>> stokeslet_kernel_;

    /// Empty container constructor to avoid initialization list complications. No way to
    /// initialize after using this constructor, so overwrite objects with full constructor.
    FiberContainer() = default;
    FiberContainer(toml::array &fiber_tables, Params &params);

    void update_derivatives();
    void update_stokeslets(double eta);
    void update_linear_operators(double dt, double eta);
    void update_cache_variables(double dt, double eta);
    void update_RHS(double dt, MatrixRef &v_on_fibers, MatrixRef &f_on_fibers);
    void apply_bc_rectangular(double dt, MatrixRef &v_on_fibers, MatrixRef &f_on_fibers);

    /// @brief get total number of nodes across fibers in the container
    /// Usually you need this to form arrays used as input later
    /// @returns total number of nodes across fibers in the container :)
    int get_local_node_count() const {
        // FIXME: This could certainly be cached
        int tot = 0;
        for (auto &fib : fibers)
            tot += fib.n_nodes_;
        return tot;
    };

    int get_global_total_fib_nodes() const;

    /// @brief Get the size of all local fibers contribution to the matrix problem solution
    int get_local_solution_size() const { return get_local_node_count() * 4; }

    /// @brief Get number of local fibers
    int get_local_count() const { return fibers.size(); };

    /// @brief Get number of local fibers
    int get_global_count() const;

    Eigen::MatrixXd generate_constant_force() const;
    Eigen::MatrixXd get_local_node_positions() const;
    Eigen::VectorXd get_RHS() const;
    Eigen::MatrixXd flow(MatrixRef &forces, MatrixRef &r_trg_external, double eta) const;
    Eigen::VectorXd matvec(VectorRef &x_all, MatrixRef &v_fib, MatrixRef &v_fib_boundary) const;
    Eigen::MatrixXd apply_fiber_force(VectorRef &x_all) const;
    Eigen::VectorXd apply_preconditioner(VectorRef &x_all) const;

    void update_boundary_conditions(Periphery &shell, bool periphery_binding_flag);

  private:
    int world_size_ = -1;
    int world_rank_;

  public:
    MSGPACK_DEFINE(fibers);
};

#endif


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Shape_detection/Efficient_RANSAC.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>

#ifdef CGAL_TEST_RANSAC_PROTOTYPE
#include <RansacShapeDetector.h>
#include <CylinderPrimitiveShapeConstructor.h>
#include <PlanePrimitiveShapeConstructor.h>
#include <CylinderPrimitiveShape.h>
#endif

#include <CGAL/IO/read_xyz_points.h>
#include <CGAL/IO/write_ply_points.h>

#include <CGAL/Real_timer.h>

#include <boost/iterator/function_output_iterator.hpp>

// Uncomment this line to run expensive test
// #define CGAL_SHAPE_DETECTION_RUN_EXPENSIVE_TESTS

namespace SD = CGAL::Shape_detection;

using Kernel = CGAL::Simple_cartesian<double>;
using Point_3 = Kernel::Point_3;
using Vector_3 = Kernel::Vector_3;

using Pwn = std::pair<Point_3, Vector_3>;
using Point_set = std::vector<Pwn>;
using Point_map = CGAL::First_of_pair_property_map<Pwn>;
using Normal_map = CGAL::Second_of_pair_property_map<Pwn>;

using RG_query = SD::Point_set::Sphere_neighbor_query<Kernel, Point_set, Point_map>;
using RG_region = SD::Point_set::Least_squares_plane_fit_region<Kernel, Point_set, Point_map, Normal_map>;
using Region_growing = SD::Region_growing<Point_set, RG_query, RG_region>;

using RANSAC_traits = SD::Efficient_RANSAC_traits<Kernel, Point_set, Point_map, Normal_map>;
using RANSAC = SD::Efficient_RANSAC<RANSAC_traits>;
using RANSAC_plane = SD::Plane<RANSAC_traits>;

void test_copied_point_cloud (const Point_set& points, std::size_t nb);

int main (int argc, char** argv)
{
  Point_set points;
  const char* ifilename = (argc > 1) ? argv[1] : "data/point_set_3.xyz";
  std::ifstream ifile(ifilename);

  if (!ifile ||
      !CGAL::IO::read_XYZ(
      ifile,
      std::back_inserter(points),
      CGAL::parameters::point_map(Point_map()).
      normal_map(Normal_map())))
  {
    std::cerr << "Reading error" << std::endl;
    return EXIT_FAILURE;
  }

  CGAL::cpp98::random_shuffle (points.begin(), points.end());

  test_copied_point_cloud (points, 1);
  test_copied_point_cloud (points, 2);
#ifdef CGAL_SHAPE_DETECTION_RUN_EXPENSIVE_TESTS
  test_copied_point_cloud (points, 5);
  test_copied_point_cloud (points, 10);
  test_copied_point_cloud (points, 20);
  test_copied_point_cloud (points, 50);
#endif

  return EXIT_SUCCESS;
}

void test_copied_point_cloud (const Point_set& original_points, std::size_t nb)
{
  CGAL::Bbox_3 bbox = CGAL::bbox_3
    (CGAL::make_transform_iterator_from_property_map (original_points.begin(), Point_map()),
     CGAL::make_transform_iterator_from_property_map (original_points.end(), Point_map()));

  std::size_t ground_truth = 6*nb*nb+1;
  std::cerr << "Ground truth   = " << ground_truth << " planes" << std::endl;

  Point_set points;
  points.reserve (nb * nb * original_points.size());
  for (std::size_t x = 0; x < nb; ++ x)
    for (std::size_t y = 0; y < nb; ++ y)
    {
      Vector_3 shift ((bbox.xmax() - bbox.xmin()) * x, (bbox.ymax() - bbox.ymin()) * y, 0);
      for (std::size_t i = 0; i < original_points.size(); ++ i)
      {
        Vector_3 noise (CGAL::get_default_random().get_double(-0.0001, 0.0001),
                        CGAL::get_default_random().get_double(-0.0001, 0.0001),
                        CGAL::get_default_random().get_double(-0.0001, 0.0001));
        points.emplace_back (original_points[i].first + shift + noise,
                             original_points[i].second);
      }
    }

  bbox = CGAL::bbox_3
    (CGAL::make_transform_iterator_from_property_map (points.begin(), Point_map()),
     CGAL::make_transform_iterator_from_property_map (points.end(), Point_map()));

  typename RANSAC::Parameters parameters;
  parameters.probability = 0.01;
  parameters.min_points = 100;
  parameters.epsilon = 0.01;
  parameters.cluster_epsilon = 0.01;
  parameters.normal_threshold = 0.95;

  CGAL::Real_timer t;
  t.start();
  RG_query rg_query (points, parameters.cluster_epsilon);
  RG_region rg_region (points, parameters.epsilon, parameters.normal_threshold, parameters.min_points);
  Region_growing region_growing (points, rg_query, rg_region);
  std::size_t nb_detected = 0;
  std::size_t nb_unassigned = 0;
  region_growing.detect (boost::make_function_output_iterator ([&](const auto&) { ++ nb_detected; }));
  region_growing.unassigned_items (boost::make_function_output_iterator ([&](const auto&) { ++ nb_unassigned; }));
  t.stop();
  std::cerr << "Region Growing = " << nb_detected << " planes (" << 1000 * t.time() << "ms)" << std::endl;

  assert (nb_detected == ground_truth);

#ifdef CGAL_SHAPE_DETECTION_RUN_EXPENSIVE_TESTS
  double timeout = 120; // 2 minutes timeout
  std::size_t nb_runs = 500;
#else
  double timeout = 60; // 1 minute timeout
  std::size_t nb_runs = 20; //
#endif

  CGAL::Real_timer timer;
  timer.start();

  std::vector<std::size_t> detected_ransac;
  std::vector<double> times_ransac;
  for (std::size_t run = 0; run < nb_runs; ++ run)
  {
    Point_set ordered_points = points;

    CGAL::Real_timer t;
    t.start();
    RANSAC ransac;
    ransac.template add_shape_factory<RANSAC_plane>();
    ransac.set_input(ordered_points);
    ransac.detect(parameters);
    t.stop();

    detected_ransac.emplace_back (ransac.shapes().size());
    times_ransac.emplace_back (t.time() * 1000);
    if (timer.time() > timeout)
    {
      nb_runs = run + 1;
      break;
    }
  }

  std::sort (detected_ransac.begin(), detected_ransac.end());
  std::sort (times_ransac.begin(), times_ransac.end());
  std::cerr << "RANSAC         = " << detected_ransac[detected_ransac.size() / 2]
            << " planes (" << times_ransac[times_ransac.size() / 2] << "ms)     (on "
            << nb_runs << " runs, planes["
            << detected_ransac.front() << ";" << detected_ransac.back() << "], time["
            << times_ransac.front() << ";" << times_ransac.back() << "])" << std::endl;

  // RANSAC should at least detect 75% of shapes
  assert (detected_ransac[detected_ransac.size() / 2] > std::size_t(0.75 * ground_truth));

#ifdef CGAL_TEST_RANSAC_PROTOTYPE
  {
    CGAL::Real_timer timer;
    double timeout = 120.; // 2 minute timeout
    timer.start();
    std::size_t nb_runs = 500;
    std::vector<std::size_t> detected_ransac;
    std::vector<double> times_ransac;
    for (std::size_t run = 0; run < nb_runs; ++ run)
    {
      PointCloud proto_points;
      proto_points.reserve (points.size());

      Point Pt;
      for (std::size_t i = 0; i < points.size(); ++i)
      {
        Pt.pos[0] = static_cast<float>(points[i].first.x());
        Pt.pos[1] = static_cast<float>(points[i].first.y());
        Pt.pos[2] = static_cast<float>(points[i].first.z());
        Pt.normal[0] = static_cast<float>(points[i].second.x());
        Pt.normal[1] = static_cast<float>(points[i].second.y());
        Pt.normal[2] = static_cast<float>(points[i].second.z());
#ifdef POINTSWITHINDEX
        Pt.index = i;
#endif
        proto_points.push_back(Pt);
      }

      //manually set bounding box!
      Vec3f cbbMin, cbbMax;
      cbbMin[0] = static_cast<float>(bbox.xmin());
      cbbMin[1] = static_cast<float>(bbox.ymin());
      cbbMin[2] = static_cast<float>(bbox.zmin());
      cbbMax[0] = static_cast<float>(bbox.xmax());
      cbbMax[1] = static_cast<float>(bbox.ymax());
      cbbMax[2] = static_cast<float>(bbox.zmax());
      proto_points.setBBox(cbbMin, cbbMax);

      // Sets parameters for shape detection.
      RansacShapeDetector::Options options;
      options.m_epsilon = parameters.epsilon;
      options.m_bitmapEpsilon = parameters.cluster_epsilon;
      options.m_normalThresh = parameters.normal_threshold;
      options.m_probability = parameters.probability;
      options.m_minSupport = parameters.min_points;

      CGAL::Real_timer t;
      t.start();
      RansacShapeDetector ransac (options); // the detector object
      ransac.Add (new PlanePrimitiveShapeConstructor());
      MiscLib::Vector<std::pair<MiscLib::RefCountPtr<PrimitiveShape>, std::size_t> > shapes; // stores the detected shapes
      ransac.Detect (proto_points, 0, proto_points.size(), &shapes);
      t.stop();

      detected_ransac.emplace_back (shapes.size());
      times_ransac.emplace_back (t.time() * 1000);
      if (timer.time() > timeout)
      {
        nb_runs = run + 1;
        break;
      }
    }

    std::sort (detected_ransac.begin(), detected_ransac.end());
    std::sort (times_ransac.begin(), times_ransac.end());
    std::cerr << "RANSAC (proto) = " << detected_ransac[detected_ransac.size() / 2]
              << " planes (" << times_ransac[times_ransac.size() / 2] << "ms)     (on "
              << nb_runs << " runs, planes["
              << detected_ransac.front() << ";" << detected_ransac.back() << "], time["
              << times_ransac.front() << ";" << times_ransac.back() << "])" << std::endl;
  }
#endif

  std::cerr << std::endl;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/repair.h>
#include <CGAL/Polygon_mesh_processing/triangulate_faces.h>
#include <boost/function_output_iterator.hpp>
#include "remeshhole.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel     Kernel;
typedef Kernel::Point_3                                         Point;
typedef CGAL::Surface_mesh<Kernel::Point_3>                     Mesh;
typedef boost::graph_traits<Mesh>::halfedge_descriptor          halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor              edge_descriptor;
typedef boost::graph_traits<Mesh>::vertex_iterator              vertex_iterator;
typedef boost::graph_traits<Mesh>::vertex_descriptor            vertex_descriptor;

struct halfedge2edge
{
  halfedge2edge(const Mesh& m, std::vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
  {}
  void operator()(const halfedge_descriptor& h) const
  {
    m_edges.push_back(edge(h, m_mesh));
  }
  const Mesh& m_mesh;
  std::vector<edge_descriptor>& m_edges;
};

void remeshHole(std::vector<QVector3D> &vertices,
    const std::vector<size_t> &hole,
    std::vector<std::vector<size_t>> &newFaces)
{
    if (hole.empty())
        return;

    Mesh mesh;

    double targetEdgeLength = 0;
    for (size_t i = 0; i < hole.size(); ++i) {
        size_t j = (i + 1) % hole.size();
        targetEdgeLength += (vertices[hole[i]] - vertices[hole[j]]).length();
    }
    targetEdgeLength /= hole.size();
    targetEdgeLength *= 1.2;

    std::vector<typename CGAL::Surface_mesh<typename Kernel::Point_3>::Vertex_index> meshFace;
    std::vector<size_t> originalIndices;
    originalIndices.reserve(hole.size());
    for (const auto &v: hole) {
        originalIndices.push_back(v);
        const auto &position = vertices[v];
        meshFace.push_back(mesh.add_vertex(Point(position.x(), position.y(), position.z())));
    }
    mesh.add_face(meshFace);

    std::vector<edge_descriptor> border;

    CGAL::Polygon_mesh_processing::triangulate_faces(mesh);

    CGAL::Polygon_mesh_processing::border_halfedges(faces(mesh),
        mesh,
        boost::make_function_output_iterator(halfedge2edge(mesh, border)));

    auto ecm = mesh.add_property_map<edge_descriptor, bool>("ecm").first;
    for (edge_descriptor e: border)
        ecm[e] = true;

    Mesh::Property_map<Mesh::Vertex_index, size_t> meshPropertyMap;
    bool created;
    boost::tie(meshPropertyMap, created) = mesh.add_property_map<Mesh::Vertex_index, size_t>("v:source", 0);

    size_t vertexIndex = 0;
    for (auto vertexIt = mesh.vertices_begin(); vertexIt != mesh.vertices_end(); vertexIt++) {
        meshPropertyMap[*vertexIt] = originalIndices[vertexIndex++];
    }

    unsigned int nb_iter = 3;

    CGAL::Polygon_mesh_processing::isotropic_remeshing(faces(mesh),
        targetEdgeLength,
        mesh,
        CGAL::Polygon_mesh_processing::parameters::number_of_iterations(nb_iter)
        .protect_constraints(true)
        .edge_is_constrained_map(ecm));

    for (auto vertexIt = mesh.vertices_begin() + vertexIndex; vertexIt != mesh.vertices_end(); vertexIt++) {
        auto point = mesh.point(*vertexIt);
        originalIndices.push_back(vertices.size());
        vertices.push_back(QVector3D(
            CGAL::to_double(point.x()),
            CGAL::to_double(point.y()),
            CGAL::to_double(point.z())
        ));
        meshPropertyMap[*vertexIt] = originalIndices[vertexIndex++];
    }

    for (const auto &faceIt: mesh.faces()) {
        CGAL::Vertex_around_face_iterator<Mesh> vbegin, vend;
        std::vector<size_t> faceIndices;
        for (boost::tie(vbegin, vend) = CGAL::vertices_around_face(mesh.halfedge(faceIt), mesh);
                vbegin != vend;
                ++vbegin) {
            faceIndices.push_back(meshPropertyMap[*vbegin]);
        }
        newFaces.push_back(faceIndices);
    }
}


#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <apriltag_ros/AprilTagDetectionArray.h>
#include <apriltag_ros/AprilTagDetection.h>
#include <Eigen/Dense>

using namespace apriltag_ros;
#define MASTER_ID 0

int g_frame_cnt = 0;
int input_frame_count = 0;

std::vector<apriltag_ros::AprilTagDetectionArray> detection_array_deque;

Eigen::Matrix3d quat2Rotmap(const geometry_msgs::Quaternion &ros_q)
{
    Eigen::Quaterniond q(ros_q.w, ros_q.x, ros_q.y, ros_q.z);
    return q.matrix();
}

Eigen::Matrix4d createTransform(const geometry_msgs::PoseWithCovarianceStamped &pc)
{
    Eigen::Affine3d transform = Eigen::Affine3d::Identity();
    Eigen::Vector3d t(pc.pose.pose.position.x, pc.pose.pose.position.y, pc.pose.pose.position.z);
    Eigen::Quaterniond q(pc.pose.pose.orientation.w,
                         pc.pose.pose.orientation.x,
                         pc.pose.pose.orientation.y,
                         pc.pose.pose.orientation.z);
    transform.rotate(q);
    transform.pretranslate(t);
    return transform.matrix();
}

void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArrayConstPtr &detection_array)
{
    // Use 500 frame detections to generate a calibration result
    if (g_frame_cnt++ > input_frame_count)
    {
        double master_size;
        std::map<int, std::vector<Eigen::Matrix4d>> rel_mats; // 某帧某个tag
        for (const AprilTagDetectionArray &detections : detection_array_deque)
        {
            auto it = std::find_if(detections.detections.begin(), detections.detections.end(), [&](const AprilTagDetection &d) {
                return d.id.front() == MASTER_ID;
            });
            // Master not detected in this detection, so this particular
            // detection is useless
            if (it == detections.detections.end())
                continue;

            // Get the master tag's rigid body transform to the camera frame
            Eigen::Matrix4d T_cm = createTransform(it->pose);
            master_size = it->size.front();

            // Get the rigid body transform of every other tag to the camera frame
            std::vector<Eigen::Matrix4d> mats;
            for (const auto &detection : detections.detections)
            {
                // Skip the master, but get its size first
                if (detection.id.front() == MASTER_ID)
                    continue;
                //We already have the rigid body transform from the master tag to the camera frame (T_cm)
                int other_id = detection.id.front();

                //Get this tag's rigid body transform to the camera frame
                Eigen::Matrix4d other_T_cm = createTransform(detection.pose);

                // Deduce this tag's rigid body transform to the master tag's frame
                auto tt = T_cm.inverse();
                Eigen::Matrix4d T_m = T_cm.inverse() * other_T_cm;

                // Save the relative position and orientation of this tag to the master tag
                rel_mats[detection.id.front()].push_back(T_m);
            }
        }

        // Compute the mean position as the initial value for the minimization problem
        auto meanTranspose = [&](const std::vector<Eigen::Matrix4d> &mats) -> Eigen::Vector3d {
            double mean_x(0), mean_y(0), mean_z(0);
            for (const auto &mat : mats)
            {
                Eigen::Affine3d a3d(mat);
                mean_x += a3d.translation().x();
                mean_y += a3d.translation().y();
                mean_z += a3d.translation().z();
            }
            mean_x /= mats.size();
            mean_y /= mats.size();
            mean_z /= mats.size();
            return Eigen::Vector3d(mean_x, mean_y, mean_z);
        };

        auto quatmult = [&](const Eigen::Quaterniond &a, const Eigen::Quaterniond &b) -> Eigen::Quaterniond {
            double w = a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z();
            double x = a.w() * b.x() + a.x() * b.w() - a.y() * b.z() + a.z() * b.y();
            double y = a.w() * b.y() + a.y() * b.w() + a.x() * b.z() - a.z() * b.x();
            double z = a.w() * b.z() - a.x() * b.y() + a.y() * b.x() + a.z() * b.w();
            return Eigen::Quaterniond(w, x, y, z);
        };

        auto meanQuaternions = [&](const std::vector<Eigen::Matrix4d> &mats) -> Eigen::Quaterniond {
            double mean_x(0), mean_y(0), mean_z(0), mean_w(0);

            for (int i = 0; i < mats.size(); ++i)
            {
                Eigen::Affine3d a4d(mats[i]);
                Eigen::Quaterniond q1(Eigen::Affine3d(mats[i]).rotation());
                for (int j = 0; j < mats.size(); ++j)
                {
                    if (i == j)
                        continue;

                    Eigen::Quaterniond q2(Eigen::Affine3d(mats[j]).rotation());
                    auto q_error = quatmult(q1.inverse(), q2);
                    double q_error_w = std::min<double>(1.0, std::max<double>(q_error.w(), -1.0));
                    if (2 * std::acos(q_error_w) >= M_PI / 2.0)
                        printf("Quaternion pair q_%d and q_%d are more than 90 degrees apart!", i, j);
                }
            }
        };

        // Compute the geometric median
        std::map<int, Eigen::Affine3d> rel_mats_median;
        for (const auto &ms : rel_mats)
        {
            // avereage transpose
            auto p_0 = meanTranspose(ms.second);
            // TODO In matlab code, it applied a optimization search algorithm to search a result
	    // In this version, use mean result only
            rel_mats_median[ms.first] = Eigen::Affine3d::Identity();
            rel_mats_median[ms.first].pretranslate(p_0);
        }

        // Compute the quaternion median
        for (const auto &ms : rel_mats)
        {
            auto q_0 = meanQuaternions(ms.second);
            Eigen::MatrixXd Q(4, ms.second.size());
            for (size_t i = 0; i < ms.second.size(); ++i)
            {
                Eigen::Affine3d a3d(ms.second[i]);
                Eigen::Quaterniond qq(a3d.rotation());
                double w = qq.w();
                double x = qq.x();
                double y = qq.y();
                double z = qq.z();
                Q.col(i) = Eigen::Vector4d(w, x, y, z);
            }

            Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(Q * Q.transpose());
            Eigen::Vector4d D = eigensolver.eigenvalues();
            Eigen::Matrix4d V = eigensolver.eigenvectors();
            double max_value(-9999999);
            int imax;
            for (int j = 0; j < 4; j++)
            {
                if (D[j] > max_value)
                {
                    imax = j;
                    max_value = D[j];
                }
            }
            Eigen::Quaterniond q(V.col(imax)[0], V.col(imax)[1], V.col(imax)[2], V.col(imax)[3]);
            if (q.w() < 0)
                q.coeffs()[0] = -q.w();
            rel_mats_median[ms.first].rotate(q);
        }

        for (const std::pair<int, Eigen::Affine3d> &trans : rel_mats_median)
        {
            std::cout << "{id: " << trans.first
                      << ", size: " << 0.1077
                      << ", x: " << trans.second.translation()[0]
                      << ", y: " << trans.second.translation()[1]
                      << ", z: " << trans.second.translation()[2];
            Eigen::Quaterniond q(trans.second.rotation());
            std::cout << ", qw: " << q.w()
                      << ", qx: " << q.x()
                      << ", qy: " << q.y()
                      << ", qz: " << q.z()
                      << "}"
                      << std::endl;
        }

        g_frame_cnt = 0;
        detection_array_deque.clear();
    }
    else
    {
        detection_array_deque.emplace_back(*detection_array);
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "calib_bundel");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");
    private_nh.param("input_frame_count", input_frame_count, 100);

    ros::Subscriber dection_sub = nh.subscribe<apriltag_ros::AprilTagDetectionArray>("/tag_detections", 1, tagDetectionCallback);

    ros::spin();
}


/**
 * by Beck on 7/12/18.
 * rotation ekf used in the real system
 */
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <queue>
#include "rm_cv/ArmorRecord.h"
#include "can_receive_msg/imu_16470.h"

using namespace std;
using namespace Eigen;

string predict_topic_in, preprocess_topic_in;
string publisher_topic, predict_pub_topic, preprocess_pub_topic;
ros::Publisher pose_pub, predict_pub, preprocess_pub;
Matrix3d rotate_for_aiming;
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

/*
 * publish the predict result in angle axis
 */
void pub_pose(std_msgs::Header header, double angle, const Vector3d& axis, ros::Publisher &pub)
{
    geometry_msgs::PoseStamped pose;
    pose.header = header;
    pose.pose.orientation.w = angle;
    pose.pose.orientation.x = axis[0];
    pose.pose.orientation.y = axis[1];
    pose.pose.orientation.z = axis[2];
    pub.publish(pose);
}

void predict_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    Vector3d camera_T_shield_rot = MatrixXd::Zero(3, 1);
    camera_T_shield_rot <<  pnp->twist.linear.x,
                            pnp->twist.linear.y,
                            pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_T_camera + rotate_for_aiming * camera_T_shield_rot;
    Vector3d T_norm = imu_T_shield.normalized();
    Vector3d x_axis = Vector3d::UnitX();
    Vector3d axis = T_norm.cross(x_axis).normalized();
    double angle = acos( T_norm.dot(x_axis) );
    AngleAxisd camera_R_shield(angle, axis);
    Quaterniond camera_q_shield(camera_R_shield);

    pub_pose(pnp->header, angle, axis, predict_pub);
}

void preprocess_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    Vector3d camera_T_shield_rot = MatrixXd::Zero(3, 1);
    camera_T_shield_rot <<  pnp->twist.linear.x,
            pnp->twist.linear.y,
            pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_T_camera + rotate_for_aiming * camera_T_shield_rot;
    Vector3d T_norm = imu_T_shield.normalized();
    Vector3d x_axis = Vector3d::UnitX();
    Vector3d axis = T_norm.cross(x_axis).normalized();
    double angle = acos( T_norm.dot(x_axis) );
    AngleAxisd camera_R_shield(angle, axis);
    Quaterniond camera_q_shield(camera_R_shield);

    pub_pose(pnp->header, angle, axis, preprocess_pub);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rotation_ekf");
    ros::NodeHandle n("~");

    n.param("predict_result", predict_topic_in, string("/prediction_kf/predict"));
    n.param("preprocessed_result", preprocess_topic_in, string("/prediction_kf/preprocessed"));
    n.param("publisher_topic", publisher_topic, string("/rotation_ekf/filtered"));
    n.param("predict_topic", predict_pub_topic, string("/rotation_ekf/predict"));
    n.param("preprocess_topic", preprocess_pub_topic, string("/rotation_ekf/preprocess"));

    ros::Subscriber s1 = n.subscribe(predict_topic_in, 10, predict_callback);
    ros::Subscriber s2 = n.subscribe(preprocess_topic_in, 10, preprocess_callback);
    pose_pub = n.advertise<geometry_msgs::PoseStamped>(publisher_topic, 100);
    predict_pub = n.advertise<geometry_msgs::PoseStamped>(predict_pub_topic, 100);
    preprocess_pub = n.advertise<geometry_msgs::PoseStamped>(preprocess_pub_topic, 100);

    rotate_for_aiming << 0, 0, 1,
       -1, 0, 0,
        0, 1, 0;
    imu_T_camera << 150, 45, -30;

    ros::Rate r(100);
    ros::spin();
}


#include <boost/graph/graphviz.hpp>
#include <gameai/point2.hpp>
#include <gameai/astar.hpp>

struct edge_label_t { typedef boost::edge_property_tag kind; };

typedef boost::property<gameai::astar_pos_t, gameai::point2<int>> vertex_pos_p;
typedef boost::property<boost::vertex_name_t, boost::graph_traits<boost::adjacency_list<>>::vertices_size_type, vertex_pos_p> vertex_p;
typedef boost::property<boost::edge_weight_t, unsigned int> edge_p;
typedef boost::no_property graph_p;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, vertex_p, edge_p, graph_p> graph_t;

int main(int argc, char **argv) {
	std::ifstream in("assets/graph.dot");

	graph_t g;
	boost::dynamic_properties dp(boost::ignore_other_properties);
	dp.property("node_id", boost::get(boost::vertex_name,    g));
	dp.property("pos",     boost::get(gameai::astar_pos_t(), g));
	dp.property("label",   boost::get(boost::edge_weight,    g));

	boost::read_graphviz(in, g, dp);

	auto astar = gameai::make_astar<gameai::distance>(g);
	BOOST_REVERSE_FOREACH(auto v, astar.path(1, 61)) {
		auto n = boost::get(boost::vertex_name, g, v->vertex);
		std::cout << n << std::endl;
	}

	return 0;
}


#pragma once
#include <Eigen/Sparse>
#include <tuple>

#include "grid.h"
#include "mtao/algebra/combinatorial.hpp"
#include "mtao/iterator/enumerate.hpp"
#include "staggered_grid_utils.hpp"

namespace mtao::geometry::grid {

// A not-so-simple staggered grid class
template <typename T, int Dim>
struct StaggeredGrid {
   public:
    constexpr static int D = Dim;
    using GridType = GridD<T, Dim>;
    using Scalar = T;
    using BBox = typename GridType::BBox;
    using Indexer = typename GridType::Indexer;
    using coord_type = typename GridType::coord_type;
    using Vec = typename GridType::Vec;
    using VecMap = typename GridType::VecMap;
    using CVecMap = typename GridType::CVecMap;
    using IVec = typename GridType::IVec;
    using IVecMap = typename GridType::IVecMap;
    using CIVecMap = typename GridType::CIVecMap;
    using StaggeredGrids =
        decltype(staggered_grid::make_grids(std::declval<GridType>()));

    const Vec& dx() const { return vertex_grid().dx(); }

    template <int N>
    constexpr static size_t form_grid_size() {
        return std::tuple_size<std::tuple_element_t<N, StaggeredGrids>>();
    }
    /*
    StaggeredGrid(const coord_type& shape): Base(shape) {
        resize_grids();
    }
    */
    template <typename U>
    StaggeredGrid(const StaggeredGrid<U, D>& o)
        : StaggeredGrid(o.vertex_grid().template cast<T>()) {}
    // the vertex grid is the "defining" type
    StaggeredGrid(const GridType& g) {
        std::get<0>(std::get<0>(m_grids)) = g;
        resize_grids();
    }
    StaggeredGrid(const coord_type& g) {
        std::get<0>(std::get<0>(m_grids)) = g;
        resize_grids();
    }
    /*
    template <typename... Args>
        StaggeredGrid(Args&&... args) {

            static_assert((std::is_integral_v<Args> && ... ));
            std::get<0>(std::get<0>(m_grids)) =
    GridType(std::forward<Args>(args)...); resize_grids();
        }
        */
    // template <typename... Args>
    //    StaggeredGrid(const coord_type& shape, Args&&... args): Base(shape,
    //    std::forward<Args>(args)...) {
    //        resize_grids();
    //    }
    StaggeredGrid() {}
    StaggeredGrid(const StaggeredGrid& other) = default;
    StaggeredGrid(StaggeredGrid&& other) = default;
    StaggeredGrid& operator=(const StaggeredGrid& other) = default;
    StaggeredGrid& operator=(StaggeredGrid&& other) = default;

    auto bbox() const { return vertex_grid().bbox(); }
    static StaggeredGrid from_bbox(BBox bb, const coord_type& shape,
                                   bool cubes = false) {
        return GridType::from_bbox(bb, shape, cubes);
    }
    auto&& origin() const { return vertex_grid().origin(); }
    template <int N, int K>
    const GridType& grid() const {
        return std::get<K>(std::get<N>(m_grids));
    }
    template <int N>
    auto&& grids() const {
        return std::get<N>(m_grids);
    }
    template <int N>
    const GridType& grid(int K) const {
        return std::get<N>(m_grids)[K];
    }
    auto&& vertex_grid() const { return grid<0, 0>(); }
    auto&& cell_grid() const { return grid<D, 0>(); }
    template <int N>
    auto offsets() const {
        return std::get<N>(m_offsets);
    }
    template <int N, int K>
    int offset() const {
        return offsets<N>()[K];
    }
    template <int N>
    int offset(int K) const {
        return offsets<N>()[K];
    }

    template <int N, int K>
    size_t staggered_index(const coord_type& idx) const {
        return offset<N, K>() + grid<N, K>().index(idx);
    }
    template <int N>
    size_t staggered_index(const coord_type& idx, int K) const {
        return offset<N>(K) + grid<N>(K).index(idx);
    }
    template <int N, int K>
    auto staggered_unindex(int idx) const {
        return grid<N, K>().unindex(idx - offset<N, K>());
    }
    template <int N>
    auto staggered_unindex(int idx, int K) const {
        return grid<N>(K).unindex(idx - offset<N>(K));
    }
    template <int N, int K>
    auto staggered_vertices() const {
        return grid<N, K>().vertices();
    }
    template <int N, int K>
    auto staggered_vertex(const coord_type& idx) const {
        return grid<N, K>().vertex(idx);
    }
    template <int N, int K>
    const coord_type& staggered_shape() const {
        return grid<N, K>().shape();
    }
    template <int N, int K>
    size_t staggered_size() const {
        return grid<N, K>().size();
    }
    template <int N>
    auto staggered_vertices(int K) const {
        return grid<N>(K).vertices();
    }
    template <int N>
    auto staggered_vertex(const coord_type& idx, int K) const {
        return grid<N>(K).vertex(idx);
    }
    template <int N>
    const coord_type& staggered_shape(int K) const {
        return grid<N>(K).shape();
    }
    template <int N>
    size_t staggered_size(int K) const {
        return grid<N>(K).size();
    }
    template <int N, int K = 0>
    auto staggered_vertex(int idx) const {
        return staggered_vertex<N, K>(staggered_unindex<N, K>(idx));
    }
    template <int N>
    auto staggered_vertex(int idx, int K) const {
        return staggered_vertex<N>(staggered_unindex<N>(idx, K), K);
    }
    template <int N, int K>
    bool staggered_valid_index(const coord_type& idx) const {
        return grid<N, K>().valid_index(idx);
    }
    template <int N>
    bool staggered_valid_index(const coord_type& idx, int K) const {
        return grid<N>(K).valid_index(idx);
    }

    auto vertex(const coord_type& idx) const {
        return staggered_vertex<0, 0>(idx);
    }
    auto vertex(int idx) const { return vertex(staggered_unindex<0, 0>(idx)); }
    auto vertices() const { return staggered_vertices<0, 0>(); }
    auto cell_vertex(const coord_type& idx) const {
        return staggered_vertex<D>(idx);
    }
    auto cell_vertex(int idx) const {
        return vertex(staggered_unindex<D, 0>(idx));
    }
    auto cell_vertices() const { return staggered_vertices<D, 0>(); }

    size_t u_index(const coord_type& idx) const {
        return staggered_index<1, 0>(idx);
    }
    size_t v_index(const coord_type& idx) const {
        return staggered_index<1, 1>(idx);
    }
    size_t w_index(const coord_type& idx) const {
        return staggered_index<1, 2>(idx);
    }
    size_t vw_index(const coord_type& idx) const {
        return staggered_index<2, 0>(idx);
    }
    size_t uw_index(const coord_type& idx) const {
        return staggered_index<2, 1>(idx);
    }
    size_t uv_index(const coord_type& idx) const {
        return staggered_index<2, 2>(idx);
    }
    size_t vertex_index(const coord_type& idx) const {
        return staggered_index<0, 0>(idx);
    }
    size_t cell_index(const coord_type& idx) const {
        return staggered_index<D, 0>(idx);
    }
    coord_type u_unindex(size_t idx) const {
        return staggered_unindex<1, 0>(idx);
    }
    coord_type v_unindex(size_t idx) const {
        return staggered_unindex<1, 1>(idx);
    }
    coord_type w_unindex(size_t idx) const {
        return staggered_unindex<1, 2>(idx);
    }
    coord_type vw_unindex(size_t idx) const {
        return staggered_unindex<2, 0>(idx);
    }
    coord_type uw_unindex(size_t idx) const {
        return staggered_unindex<2, 1>(idx);
    }
    coord_type uv_unindex(size_t idx) const {
        return staggered_unindex<2, 2>(idx);
    }
    coord_type vertex_unindex(size_t idx) const {
        return staggered_unindex<0, 0>(idx);
    }
    coord_type cell_unindex(size_t idx) const {
        return staggered_unindex<D, 0>(idx);
    }

    const coord_type& u_shape() const { return staggered_shape<1, 0>(); }
    const coord_type& v_shape() const { return staggered_shape<1, 1>(); }
    const coord_type& w_shape() const { return staggered_shape<1, 2>(); }
    const coord_type& vw_shape() const { return staggered_shape<2, 0>(); }
    const coord_type& uw_shape() const { return staggered_shape<2, 1>(); }
    const coord_type& uv_shape() const { return staggered_shape<2, 2>(); }
    const coord_type& vertex_shape() const { return staggered_shape<0, 0>(); }
    const coord_type& cell_shape() const { return staggered_shape<D, 0>(); }

    size_t u_size() const { return staggered_size<1, 0>(); }
    size_t v_size() const { return staggered_size<1, 1>(); }
    size_t w_size() const { return staggered_size<1, 2>(); }
    size_t vw_size() const { return staggered_size<2, 0>(); }
    size_t uw_size() const { return staggered_size<2, 1>(); }
    size_t uv_size() const { return staggered_size<2, 2>(); }
    size_t vertex_size() const { return staggered_size<0, 0>(); }
    size_t cell_size() const { return staggered_size<D, 0>(); }

    bool u_valid_index(const coord_type& idx) const {
        return staggered_valid_index<1, 0>(idx);
    }
    bool v_valid_index(const coord_type& idx) const {
        return staggered_valid_index<1, 1>(idx);
    }
    bool w_valid_index(const coord_type& idx) const {
        return staggered_valid_index<1, 2>(idx);
    }
    bool vw_valid_index(const coord_type& idx) const {
        return staggered_valid_index<2, 0>(idx);
    }
    bool uw_valid_index(const coord_type& idx) const {
        return staggered_valid_index<2, 1>(idx);
    }
    bool uv_valid_index(const coord_type& idx) const {
        return staggered_valid_index<2, 2>(idx);
    }
    bool vertex_valid_index(const coord_type& idx) const {
        return staggered_valid_index<0, 0>(idx);
    }
    bool cell_valid_index(const coord_type& idx) const {
        return staggered_valid_index<D, 0>(idx);
    }

    template <int D>
    std::array<T, combinatorial::nCr(Dim, D)> form_volumes() const {
        std::array<T, combinatorial::nCr(Dim, D)> R;
        if constexpr (D == 0) {
            return {{T(1)}};
        } else if constexpr (D == 1) {
            std::copy(dx().data(), dx().data() + dx().size(), R.begin());
        } else if constexpr (D == Dim - 1) {
            std::fill(R.begin(), R.end(), 1);
            for (int i = 0; i < Dim; ++i) {
                for (int j = 1; j < Dim; ++j) {
                    R[i] *= dx()((i + j) % Dim);
                }
            }
        } else if constexpr (D == Dim) {
            return {{dx().prod()}};
        }
        return R;
    }
    std::vector<T> form_volumes(int D) const {
        int size = combinatorial::nCr(Dim, D);
        std::vector<T> R(size, 1);
        if (D == 0) {
            return {T(1)};
        } else if (D == 1) {
            std::copy(dx().data(), dx().data() + dx().size(), R.begin());
        } else if (D == Dim - 1) {
            std::fill(R.begin(), R.end(), 1);
            for (int i = 0; i < Dim; ++i) {
                for (int j = 1; j < Dim; ++j) {
                    R[i] *= dx()((i + j) % Dim);
                }
            }
        } else if (D == Dim) {
            return {dx().prod()};
        }
        return R;
    }

    template <int D>
    size_t form_size() const {
        auto&& gs = std::get<D>(m_offsets);
        using U = types::remove_cvref_t<decltype(gs)>;
        return gs[std::tuple_size<U>() - 1];
    }
    size_t edge_size() const { return form_size<1>(); }
    size_t flux_size() const { return form_size<D - 1>(); }

    template <int D>
    int form_type(int index) const {
        if (index < 0) {
            return -1;
        }
        using namespace iterator;
        auto&& ofs = offsets<D>();
        size_t result = 0;
        using U = types::remove_cvref_t<decltype(ofs)>;
        for (auto&& [i, v] : enumerate(reverse(ofs))) {
            if (index >= v) {
                result = std::tuple_size<U>() - i - 1;
                break;
            }
        }
        if (result >= ofs.size()) {
            return -1;
        }
        return result;
    }

    size_t edge_type(int index) const { return form_type<1>(index); }
    template <int D>
    auto form_unindex(int idx) const {
        int ft = form_type<D>(idx);
        return std::make_tuple(staggered_unindex<D>(idx, ft), ft);
    }
    template <typename Derived>
    auto coord(const Eigen::MatrixBase<Derived>& v) const {
        return vertex_grid().coord(v);
    }
    template <int N>
    Eigen::SparseMatrix<T> boundary() const {
        int rows = form_size<N - 1>();
        int cols = form_size<N>();
        std::vector<Eigen::Triplet<T>> trips;
        for (auto&& [K, grids] : iterator::enumerate(std::get<N>(m_grids))) {
            std::bitset<D> difference_mask = combinatorial::nCr_mask<D>(N, K);
            grid<N>(K).loop([&, K = K](const coord_type& c) {
                int col = staggered_index<N>(c, K);
                // auto s = staggered_shape<N>(K);
                masked_difference_looper(
                    difference_mask, c,
                    [&](const coord_type& l, const coord_type& u, int d) {
                        std::bitset mybs = difference_mask;
                        mybs[d] = 0;
                        int dim = combinatorial::nCr_unmask<D>(N - 1, mybs);

                        // auto s = staggered_shape<N-1>(dim);
                        int lrow =
                            static_cast<int>(staggered_index<N - 1>(l, dim));
                        int urow =
                            static_cast<int>(staggered_index<N - 1>(u, dim));
                        trips.emplace_back(lrow, col, T{-1});
                        trips.emplace_back(urow, col, T{1});
                    });
            });
        }

        Eigen::SparseMatrix<T> A(rows, cols);
        A.setFromTriplets(trips.begin(), trips.end());
        return A;
    }
    template <int N, typename Func>
    static void cell_vertex_looper(int K, const coord_type& c, Func&& f) {
        staggered_grid::internal::bitmask_looper(
            combinatorial::nCr_mask<D>(N, K), [&](const std::bitset<D>& bs) {
                coord_type cc = c;
                for (int j = 0; j < D; ++j) {
                    cc[j] += bs[j];
                }
                f(cc);
            });
    }
    template <int N, typename Func>
    static void cell_dual_vertex_looper(int K, const coord_type& c, Func&& f) {
        staggered_grid::internal::bitmask_looper(
            !combinatorial::nCr_mask<D>(N, K), [&](const std::bitset<D>& bs) {
                coord_type cc = c;
                for (int j = 0; j < D; ++j) {
                    cc[j] -= bs[j];
                }
                f(cc);
            });
    }

    template <typename Func>
    static void masked_difference_looper(const std::bitset<D>& mask,
                                         const coord_type& c, Func&& f) {
        coord_type cc = c;
        for (int j = 0; j < D; ++j) {
            if (mask[j]) {
                cc[j]++;
                const coord_type& ccc = cc;
                f(c, ccc, j);
                cc[j]--;
            }
        }
    }
    // f( lower_index_boundary, higher_index_boundary, index) as a centered
    // difference stencil for <1>::instance boundary_cell_looper<1>(0,{N+.5},f)
    // calls
    //          f({N},{N+1},0)
    // for <2>::instance boundary_cell_looper<2>(1,{N+.5,M+.5},f) calls
    //          f({N,M+.5},{N+1,M+.5},0)
    //          f({N+.5,M},{N+.5,M+1},1)
    template <int N, typename Func>
    static void cell_boundary_looper(int K, const coord_type& c, Func&& f) {
        auto comb = combinatorial::nCr_mask<D>(N, K);
        masked_difference_looper(comb, c, f);
    }

    // opposite of the above
    template <int N, typename Func>
    static void dual_cell_boundary_looper(int K, const coord_type& c,
                                          Func&& f) {
        coord_type cc = c;
        auto comb = !combinatorial::nCr_mask<D>(N, K);
        for (int j = 0; j < D; ++j) {
            if (comb[j]) {
                cc[j]--;
                const coord_type& ccc = cc;
                f(ccc, c, j);
                cc[j]++;
            }
        }
    }

    template <int N>
    auto form_vertices(int K, const coord_type& c) const {
        std::array<int, 1 << N> ret;
        auto it = ret.begin();
        cell_vertex_looper<N>(
            K, c, [&](const coord_type& c) { *(it++) = vertex_index(c); });
        return ret;
    }
    template <int D>
    auto form_vertices(int idx) const {
        int K = form_type<D>(idx);
        auto coord = staggered_unindex<D>(idx, K);
        return form_vertices<D>(K, coord);
    }
    auto edge(int K, const coord_type& c) const -> std::array<int, 2> {
        return form_vertices<1>(K, c);
    }
    auto edge(int idx) const { return form_vertices<1>(idx); }

   private:
    void resize_grids() {
        m_grids = staggered_grid::make_grids(vertex_grid());
        m_offsets = staggered_grid::staggered_grid_offsets(vertex_shape());
    }
    StaggeredGrids m_grids;
    decltype(staggered_grid::staggered_grid_offsets(
        std::declval<coord_type>())) m_offsets;
};

using StaggeredGrid2f = StaggeredGrid<float, 2>;
using StaggeredGrid3f = StaggeredGrid<float, 3>;
using StaggeredGrid2d = StaggeredGrid<double, 2>;
using StaggeredGrid3d = StaggeredGrid<double, 3>;
}  // namespace mtao::geometry::grid


/******************************************************************
 *
 * Round for C++
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <stdio.h>
#include <stdlib.h>

#include <algorithm>
#include <ctime>
#include <climits>
#include <boost/random.hpp>

#include <round/common/Random.h>

#if defined(ROUND_HAVE_BOOST_RANDOM)
static boost::random::mt19937 *gRandEngine = NULL;
#else
static boost::mt19937 *gRandEngine = NULL;
#endif

Round::Random::Random() {
  initialize(0, RAND_MAX);
}

Round::Random::Random(unsigned int min, unsigned int max) {
  initialize(std::min(min, max), std::max(min, max));
}

Round::Random::~Random() {
}

void Round::Random::initialize(unsigned int min, unsigned int max) {
  this->minRange = min;
  this->maxRange = max;

  if (!gRandEngine) {
    uint32_t seed = std::time(0) % std::numeric_limits<uint32_t>::max();
#if defined(ROUND_HAVE_BOOST_RANDOM)
    gRandEngine = new boost::random::mt19937(seed);
#else
    gRandEngine = new boost::mt19937(seed);
#endif
  }
}

unsigned int Round::Random::rand() {
#if defined(ROUND_HAVE_BOOST_RANDOM)
  boost::random::uniform_int_distribution<> randDist(this->minRange, this->maxRange);
#else
  boost::uniform_smallint<> randDist(this->minRange, this->maxRange);
#endif
  return randDist(*gRandEngine);
}


#include <iostream>
#include <list>
#include <experimental/filesystem>
#include <fstream>
#include <map>
#include <chrono>
#include <cmath>
#include <iomanip>
#include <armadillo>
#include <omp.h>
#include <cassert>

#include "../../lib/json.hpp"
#include "../exciton_transfer/cnt.h"
#include "../helper/prepare_directory.hpp"
#include "../helper/progress.hpp"
#include "monte_carlo.h"

namespace mc
{

  // high level method to calculate proper scattering table
  std::vector<std::vector<scattering_struct>> monte_carlo::create_scattering_table(nlohmann::json j) {
    assert(j.count("rate type")>0);

    std::string rate_type = j["rate type"].get<std::string>();

    std::cout << "\ninitializing scattering table with " << rate_type << "..." << std::endl;

    if (rate_type == "davoody") {

      // get the parent directory for cnts
      std::string parent_directory = j["cnts"]["directory"];
      j["cnts"].erase("directory");
      j["cnts"].erase("comment");

      // create excitons and calculate exciton dispersions
      std::vector<cnt> cnts;
      cnts.reserve(j["cnts"].size()); // this is reservation of space is crucial to ensure we do not move
                                      // cnts, since the move constructor is not implemented yet

      for (const auto& j_cnt : j["cnts"]) {
        cnts.emplace_back(cnt(j_cnt, parent_directory));
        cnts.back().calculate_exciton_dispersion();
      };

	  std::vector<std::vector<scattering_struct>> all_tables(size(cnts));
	  for (int i = 0; i < size(cnts); i++) {
		  all_tables[i] = std::vector<scattering_struct>(size(cnts));
		  for (int j = 0; j < size(cnts); j++) {
			  all_tables[i][j] = create_davoody_scatt_table(cnts[i], cnts[j]);
		  }
	  }

	  return all_tables;
    }

    /*if (rate_type == "forster") {
      return create_forster_scatt_table(1.e15, 1.4e9);
    }

    if (rate_type == "wong") {
      return create_forster_scatt_table(1.e13, 1.4e9);
    }*/

    throw std::invalid_argument("rate type must be one of the following: \"davoody\", \"forster\", \"wong\"");

  };

  // method to calculate scattering rate via davoody et al. method
  scattering_struct monte_carlo::create_davoody_scatt_table(const cnt& d_cnt, const cnt& a_cnt) {
    auto zshift_prop = _json_prop["zshift [m]"];
    arma::vec z_shift = arma::linspace<arma::vec>(zshift_prop[0], zshift_prop[1], zshift_prop[2]);

    auto axis_shift_prop_1 = _json_prop["axis shift 1 [m]"];
    arma::vec axis_shift_1 = arma::linspace<arma::vec>(axis_shift_prop_1[0], axis_shift_prop_1[1], axis_shift_prop_1[2]);

    auto axis_shift_prop_2 = _json_prop["axis shift 2 [m]"];
    arma::vec axis_shift_2 = arma::linspace<arma::vec>(axis_shift_prop_2[0], axis_shift_prop_2[1], axis_shift_prop_2[2]);

    auto theta_prop = _json_prop["theta [degrees]"];
    arma::vec theta = arma::linspace<arma::vec>(theta_prop[0], theta_prop[1], theta_prop[2])*(constants::pi/180);

    arma::field<arma::cube> rate(theta.n_elem);
    rate.for_each([&](arma::cube& c){c.zeros(z_shift.n_elem, axis_shift_1.n_elem, axis_shift_2.n_elem);});

    exciton_transfer ex_transfer(d_cnt, a_cnt);

    ex_transfer.save_atom_locations(_output_directory.path(), {0, 0}, 1.5e-9, 0, ".0_angle");
    ex_transfer.save_atom_locations(_output_directory.path(), {0, 0}, 1.5e-9, constants::pi / 2, ".90_angle");
    ex_transfer.save_atom_locations(_output_directory.path(), {0, 0}, 1.5e-9, constants::pi, ".180_angle");

    #ifdef DEBUG_CHECK_RATES_SYMMETRY
    {
      double zsh = 1.5e-9;
      double ash1 = 0;
      double ash2 = 0;

      double th = 0;
      double r = ex_transfer.first_order(zsh, {ash1, ash2}, th, false);
      std::cout << "rate(" << th << ") = " << r << std::endl;

      th = constants::pi;
      r = ex_transfer.first_order(zsh, {ash1, ash2}, th, false);
      std::cout << "rate(" << th << ") = " << r << std::endl;

      std::exit(0);
    }
    #endif

    // progress_bar prog(theta.n_elem*z_shift.n_elem*axis_shift_1.n_elem*axis_shift_2.n_elem,"create davoody scattering table");
    progress_bar prog(theta.n_elem * z_shift.n_elem * axis_shift_1.n_elem * axis_shift_2.n_elem, "create davoody scattering table");

    #pragma omp parallel
    {
      double th, zsh, ash1, ash2;

      #pragma omp for
      for (unsigned i_th = 0; i_th<theta.n_elem; ++i_th) {

        th = theta(i_th);
        for (unsigned i_zsh = 0; i_zsh < z_shift.n_elem; ++i_zsh) {
          zsh = z_shift(i_zsh);
          for (unsigned i_ash1 = 0; i_ash1 < axis_shift_1.n_elem; ++i_ash1) {
            ash1 = axis_shift_1(i_ash1);
            for (unsigned i_ash2 = 0; i_ash2 < axis_shift_2.n_elem; ++i_ash2) {
              ash2 = axis_shift_2(i_ash2);
              // prog.step();
              rate(i_th)(i_zsh, i_ash1, i_ash2) = ex_transfer.first_order(zsh, {ash1, ash2}, th, false);

              #pragma omp critical
              {
                prog.step();
              }
            }
          }
        }

      }
    }

    scattering_struct scat_table(rate,theta,z_shift,axis_shift_1,axis_shift_2);

    double max_rate = 0;
    double min_rate = 10e15;
    rate.for_each([&min_rate](arma::cube& c) { min_rate = min_rate < c.min() ? min_rate : c.min(); });
    rate.for_each([&max_rate](arma::cube& c) { max_rate = max_rate > c.max() ? max_rate : c.max(); });

    std::cout << std::endl
              << "max rate in davoody scattering table: " << max_rate << " [1/s]" << std::endl
              << "min rate in davoody scattering table: " << min_rate << " [1/s]"
              << std::endl
              << std::endl;

    // std::string filename(_output_directory.path() / "davoody_scat_rates.dat");
    scat_table.save(_output_directory.path());

    return scat_table;
  };

  // method to calculate scattering rate via forster method
  scattering_struct monte_carlo::create_forster_scatt_table(double gamma_0, double r_0) {
    auto zshift_prop = _json_prop["zshift [m]"];
    arma::vec z_shift = arma::linspace<arma::vec>(zshift_prop[0], zshift_prop[1], zshift_prop[2]);

    auto axis_shift_prop_1 = _json_prop["axis shift 1 [m]"];
    arma::vec axis_shift_1 = arma::linspace<arma::vec>(axis_shift_prop_1[0], axis_shift_prop_1[1], axis_shift_prop_1[2]);

    auto axis_shift_prop_2 = _json_prop["axis shift 2 [m]"];
    arma::vec axis_shift_2 = arma::linspace<arma::vec>(axis_shift_prop_2[0], axis_shift_prop_2[1], axis_shift_prop_2[2]);

    auto theta_prop = _json_prop["theta [degrees]"];
    arma::vec theta = arma::linspace<arma::vec>(theta_prop[0], theta_prop[1], theta_prop[2])*(constants::pi/180);

    arma::field<arma::cube> rate(theta.n_elem);
    rate.for_each([&](arma::cube& c){c.zeros(z_shift.n_elem, axis_shift_1.n_elem, axis_shift_2.n_elem);});

    progress_bar prog(theta.n_elem*z_shift.n_elem*axis_shift_1.n_elem*axis_shift_2.n_elem,"create forster scattering table");

    unsigned i_th=0;
    for (const auto& th: theta) {
      unsigned i_zsh=0;
      for (const auto& zsh: z_shift) {
        unsigned i_ash1=0;
        for (const auto& ash1: axis_shift_1) {
          unsigned i_ash2=0;
          for (const auto& ash2: axis_shift_2) {
            prog.step();
            arma::vec r1 = {ash1, 0, 0};
            arma::vec r2 = {ash2*std::cos(th), ash2*std::sin(th), zsh};
            arma::vec dR = r1-r2;
            double angle_factor = std::cos(th)-3*arma::dot(arma::normalise(r1),arma::normalise(dR))*arma::dot(arma::normalise(r2),arma::normalise(dR));
            rate(i_th)(i_zsh,i_ash1,i_ash2) = gamma_0*std::pow(angle_factor,2)*std::pow(1.e-9/arma::norm(dR),6);
            i_ash2++;
          }
          i_ash1++;
        }
        i_zsh++;
      }
      i_th++;
    }

    scattering_struct scat_table(rate,theta,z_shift,axis_shift_1,axis_shift_2);

    return scat_table;
  };

  // slice the domain into n sections in each direction, and return a list of scatterers in the center region as the injection region
  std::vector<const scatterer *> monte_carlo::injection_region(const std::vector<scatterer> &all_scat, const domain_t domain, const int n) {
    assert((n > 0) && (n % 2 == 1));

    double xmin = domain.first(0), ymin = domain.first(1), zmin = domain.first(2);
    double xmax = domain.second(0), ymax = domain.second(1), zmax = domain.second(2);
    double dx = (xmax - xmin) / double(n), dy = (ymax - ymin) / double(n), dz = (zmax - zmin) / double(n);

    std::vector<double> x, y, z;

    for (int i = 0; i <= n; ++i) {
      x.push_back(double(i) * dx + xmin);
      y.push_back(double(i) * dy + ymin);
      z.push_back(double(i) * dz + zmin);
    }

    std::vector<const scatterer *> inject_list;

    for (const auto& s : all_scat) {
      if (x[n / 2] <= s.pos(0) && s.pos(0) <= x[n / 2 + 1] &&
          y[n / 2] <= s.pos(1) && s.pos(1) <= y[n / 2 + 1] &&
          z[n / 2] <= s.pos(2) && s.pos(2) <= z[n / 2 + 1])
        inject_list.push_back(&s);
    }

    return inject_list;
  }

  // slice the domain into n sections in each direction, and return the domain that leaves only 1 section from each side
  monte_carlo::domain_t monte_carlo::get_removal_domain(const monte_carlo::domain_t domain, const int n) {
    assert((n > 1));

    double xmin = domain.first(0), ymin = domain.first(1), zmin = domain.first(2);
    double xmax = domain.second(0), ymax = domain.second(1), zmax = domain.second(2);
    double dx = (xmax - xmin) / double(n), dy = (ymax - ymin) / double(n), dz = (zmax - zmin) / double(n);

    std::vector<double> x, y, z;

    for (int i = 0; i <= n; ++i) {
      x.push_back(double(i) * dx + xmin);
      y.push_back(double(i) * dy + ymin);
      z.push_back(double(i) * dz + zmin);
    }

    domain_t removal_domain;
    removal_domain.first = {x[1], y[1], z[1]};
    removal_domain.second = {x[n-1], y[n-1], z[n-1]};

    return removal_domain;
  }

  // initialize the simulation condition to calculate diffusion coefficient using green-kubo approach
  void monte_carlo::kubo_init() {
    // set maximum hopping radius
    _max_hopping_radius = double(_json_prop["max hopping radius [m]"]);
    std::cout << "maximum hopping radius: " << _max_hopping_radius * 1.e9 << " [nm]\n";

    _particle_velocity = _json_prop["exciton velocity [m/s]"];
    std::cout << "exciton velocity [m/s]: " << _particle_velocity << std::endl;

    //_scat_tables = create_scattering_table(_json_prop);
    _all_scat_list = create_scatterers(_input_directory.path());

    domain_t d = find_simulation_domain();
    std::ios::fmtflags f(std::cout.flags()); // save cout flags to be reset after printing
    std::cout << std::fixed << std::showpos;
    std::cout << "\n"
              << "simulation domain BEFORE trimming:\n"
              << "    x (" << d.first(0) * 1e9 << " , " << d.second(0) * 1e9 << ") [nm]\n"
              << "    y (" << d.first(1) * 1e9 << " , " << d.second(1) * 1e9 << ") [nm]\n"
              << "    z (" << d.first(2) * 1e9 << " , " << d.second(2) * 1e9 << ") [nm]\n"
              << std::endl;
    std::cout.flags(f); // reset the cout flags

    limit_t xlim = _json_prop["trim limits"]["xlim"];
    limit_t ylim = _json_prop["trim limits"]["ylim"];
    limit_t zlim = _json_prop["trim limits"]["zlim"];

    trim_scats(xlim, ylim, zlim, _all_scat_list);

    _domain = find_simulation_domain();
    f = std::cout.flags(); // save cout flags to be reset after printing
    std::cout << std::fixed << std::showpos;
    std::cout << "\n"
              << "simulation domain AFTER trimming:\n"
              << "    x (" << _domain.first(0) * 1e9 << " , " << _domain.second(0) * 1e9 << ") [nm]\n"
              << "    y (" << _domain.first(1) * 1e9 << " , " << _domain.second(1) * 1e9 << ") [nm]\n"
              << "    z (" << _domain.first(2) * 1e9 << " , " << _domain.second(2) * 1e9 << ") [nm]\n"
              << std::endl;
    std::cout.flags(f); // reset the cout flags

    std::cout << "total number of scatterers: " << _all_scat_list.size() << std::endl;

    _quenching_list = create_quenching_sites(_all_scat_list, 10000);
    //set_scat_table(_scat_tables[0][0], _all_scat_list);

    create_scatterer_buckets(_domain, _max_hopping_radius, _all_scat_list, _scat_buckets, _quenching_list, _q_buckets);
    _scat_tables = create_scattering_table(_json_prop);
    set_scat_table(_scat_tables[0][0], _all_scat_list);
    set_max_rate(_max_hopping_radius, _all_scat_list);

    int n = _json_prop["number of sections for injection region"];
    _inject_scats = injection_region(_all_scat_list, _domain, n);
    _removal_domain = get_removal_domain(_domain, n);

    _max_time = _json_prop["maximum time for kubo simulation [seconds]"];
  };

  // create particles for kubo simulation
  void monte_carlo::kubo_create_particles() {
    int n_particle = _json_prop["number of particles for kubo simulation"];
    for (int i=0; i<n_particle; ++i) {
      int dice = std::rand() % _inject_scats.size();
      const scatterer *s = _inject_scats[dice];
      arma::vec pos = s->pos();
      _particle_list.push_back(particle(pos, s, _particle_velocity));
      _particle_list.back().set_init_pos(pos);
    }
  }

  // step the simulation in time
  void monte_carlo::kubo_step(double dt) {
    #pragma omp parallel
    {
      #pragma omp for
      for (unsigned i = 0; i < _particle_list.size(); ++i) {
        particle& p = _particle_list[i];

        p.step(dt, _all_scat_list, _max_hopping_radius);

        p.update_delta_pos();

        if (arma::any(p.pos()<_removal_domain.first) || arma::any(_removal_domain.second < p.pos())){
          int dice = std::rand() % _inject_scats.size();
          const scatterer* s = _inject_scats[dice];
          arma::vec pos = s->pos();
          p.set_pos(pos);
          p.set_scatterer(s);
        }
      }
    }

    // increase simulation time
    _time += dt;
  };

  // save the displacement of individual particles in kubo simulation
  void monte_carlo::kubo_save_individual_particle_dispalcements() {
    if (! _displacement_file_x.is_open()) {
      _displacement_file_x.open(_output_directory.path() / "particle_dispalcement.x.dat", std::ios::out);
      _displacement_file_y.open(_output_directory.path() / "particle_dispalcement.y.dat", std::ios::out);
      _displacement_file_z.open(_output_directory.path() / "particle_dispalcement.z.dat", std::ios::out);

      _displacement_file_x << std::showpos << std::scientific;
      _displacement_file_y << std::showpos << std::scientific;
      _displacement_file_z << std::showpos << std::scientific;

      _displacement_file_x << "time";
      _displacement_file_y << "time";
      _displacement_file_z << "time";
      for (int i=0; i<int(_particle_list.size()); ++i){
        _displacement_file_x << "," << i;
        _displacement_file_y << "," << i;
        _displacement_file_z << "," << i;
      }
      _displacement_file_x << std::endl;
      _displacement_file_y << std::endl;
      _displacement_file_z << std::endl;
    }

    _displacement_file_x << time();
    _displacement_file_y << time();
    _displacement_file_z << time();

    for (const auto& p: _particle_list) {
      _displacement_file_x << "," << p.delta_pos(0);
      _displacement_file_y << "," << p.delta_pos(1);
      _displacement_file_z << "," << p.delta_pos(2);
    }
    _displacement_file_x << std::endl;
    _displacement_file_y << std::endl;
    _displacement_file_z << std::endl;
  };

  void monte_carlo::kubo_save_avg_dispalcement_squared() {
    if (!_displacement_squard_file.is_open()) {
      _displacement_squard_file.open(_output_directory.path() / "particle_dispalcement.avg.squared.dat", std::ios::out);

      _displacement_squard_file << std::showpos << std::scientific;

      _displacement_squard_file << "# this file contains the average of dx^2, dy^2, and dz^2 of the particle ensemble over time" << std::endl
                                << "# number of particles: " << _particle_list.size() << std::endl
                                << std::endl;

      _displacement_squard_file << "time,x,y,z" << std::endl;
    }

    double avg_x2=0, avg_y2=0, avg_z2=0;

    for (const auto& p : _particle_list) {
      avg_x2 += std::pow(p.delta_pos(0), 2);
      avg_y2 += std::pow(p.delta_pos(1), 2);
      avg_z2 += std::pow(p.delta_pos(2), 2);
    }

    avg_x2 /= double(_particle_list.size());
    avg_y2 /= double(_particle_list.size());
    avg_z2 /= double(_particle_list.size());

    _displacement_squard_file << time() << "," << avg_x2 << "," << avg_y2 << "," << avg_z2 << std::endl;
  }

  void monte_carlo::kubo_save_diffusion_tensor(){
    if (!_diffusion_tensor_file.is_open()) {
      _diffusion_tensor_file.open(_output_directory.path() / "particle_diffusion_tensor.dat", std::ios::out);

     _diffusion_tensor_file << std::showpos << std::scientific;

      _diffusion_tensor_file << "# this file contains the diffusion tensor Dij of the particle ensemble over time" << std::endl
                                << "# number of particles: " << _particle_list.size() << std::endl
                                << std::endl;

      _diffusion_tensor_file << "time,Dxx,Dxy,Dxz,Dyy,Dyz,Dzz" << std::endl;
    }

    _diffusion_tensor_file << time();

    double Dij, D1, D2, D3;
    for(int i = 0; i < 3; i++){
      for(int j = i; j < 3; j++){
        for(const auto& p : _particle_list){
          D1 += p.pos(i)*p.pos(j);
          D2 += p.pos(i);
          D3 += p.pos(j);
        }
        Dij = (D1 + D2 * D3)/(double)(_particle_list.size());
        Dij /= double(2 * double(time()));
        _diffusion_tensor_file << "," << Dij;
      }
    }
    _diffusion_tensor_file << std::endl;
  }

  void monte_carlo::kubo_save_diffusion_length() {
    if (!_diffusion_length_file.is_open()) {
      _diffusion_length_file.open(_output_directory.path() / "particle_diffusion_length.dat", std::ios::out);

      _diffusion_length_file << std::showpos << std::scientific;

      _diffusion_length_file << "# this file contains the diffusion length in x,y,z of the particle ensemble over time" << std::endl
                                << "# number of particles: " << _particle_list.size() << std::endl
                                << std::endl;

      _diffusion_length_file << "x,y,z" << std::endl;
    }

    for (const auto& p : _particle_list) {
      _diffusion_length_file << p.diff_len(0) << "," << p.diff_len(1) << "," << p.diff_len(2) << std::endl;
    }

  }

} // end of namespace mc


#ifndef STAN_MATH_PRIM_FUN_GAMMA_Q_HPP
#define STAN_MATH_PRIM_FUN_GAMMA_Q_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{gamma\_q}(a, z) =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     Q(a, z) & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_q}(a, z)}{\partial a} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, Q(a, z)}{\partial a} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_q}(a, z)}{\partial z} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, Q(a, z)}{\partial z} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   Q(a, z)=\frac{1}{\Gamma(a)}\int_z^\infty t^{a-1}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, Q(a, z)}{\partial a} =
 -\frac{\Psi(a)}{\Gamma^2(a)}\int_z^\infty t^{a-1}e^{-t}dt
   + \frac{1}{\Gamma(a)}\int_z^\infty (a-1)t^{a-2}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, Q(a, z)}{\partial z} = -\frac{z^{a-1}e^{-z}}{\Gamma(a)}
   \f]
   * @throws domain_error if x is at pole
 */
inline double gamma_q(double x, double a) { return boost::math::gamma_q(x, a); }

}  // namespace math
}  // namespace stan
#endif


/*
 * @Description:
 * @Author: Ren Qian
 * @Date: 2020-03-01 18:35:19
 */

#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_

#include <string>
#include <deque>

#include <Eigen/Dense>

// data input:
#include "lidar_localization/sensor_data/key_frame.hpp"

#include "lidar_localization/models/pre_integrator/imu_pre_integrator.hpp"
#include "lidar_localization/models/pre_integrator/odo_pre_integrator.hpp"

namespace lidar_localization {

class InterfaceGraphOptimizer {
public:
    virtual ~InterfaceGraphOptimizer() {}

    // 优化
    virtual bool Optimize() = 0;

    // 输入、输出数据
    virtual int GetNodeNum() = 0;
    // 添加节点、边、鲁棒核
    virtual void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size) = 0;

    // SE3:
    virtual bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) = 0;
    virtual void AddSe3Node(
      const Eigen::Isometry3d &pose, const bool need_fix
    ) = 0;
    virtual void AddSe3Edge(
      const int vertex_index1, const int vertex_index2,
      const Eigen::Isometry3d &relative_pose, const Eigen::VectorXd &noise
    ) = 0;
    virtual void AddSe3PriorXYZEdge(
      const int se3_vertex_index,
      const Eigen::Vector3d &xyz, const Eigen::VectorXd &noise
    ) = 0;
    virtual void AddSe3PriorQuaternionEdge(
      const int se3_vertex_index,
      const Eigen::Quaterniond &quat, const Eigen::VectorXd &noise
    ) = 0;

    // LIO state:
    virtual bool GetOptimizedKeyFrame(std::deque<KeyFrame> &optimized_key_frames) = 0;
    /**
     * @brief  add vertex for LIO key frame
     * @param  lio_key_frame, LIO key frame with (pos, ori, vel, b_a and b_g)
     * @param  need_fix, shall the vertex be fixed to eliminate trajectory estimation ambiguity
     * @return true if success false otherwise
     */
    virtual void AddPRVAGNode(
      const KeyFrame &lio_key_frame, const bool need_fix
    ) = 0;
    virtual void AddPRVAGRelativePoseEdge(
      const int vertex_index_i, const int vertex_index_j,
      const Eigen::Matrix4d &relative_pose, const Eigen::VectorXd &noise
    ) = 0;
    virtual void AddPRVAGPriorPosEdge(
      const int vertex_index,
      const Eigen::Vector3d &pos, const Eigen::Vector3d &noise
    ) = 0;
    virtual void AddPRVAGIMUPreIntegrationEdge(
      const int vertex_index_i, const int vertex_index_j,
      const IMUPreIntegrator::IMUPreIntegration &imu_pre_integration
    ) = 0;
    virtual void AddPRVAGOdoPreIntegrationEdge(
      const int vertex_index_i, const int vertex_index_j,
      const OdoPreIntegrator::OdoPreIntegration &odo_pre_integration
    ) = 0;

    void SetMaxIterationsNum(int max_iterations_num);

protected:
    int max_iterations_num_ = 512;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/config.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_inf Inf
 *
 * \par Description
 * Constant Inf
 * \arg Maximum value of the type for integer types
 * \arg \f$= \infty \f$ for floating points
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/inf.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::inf_(A0)>::type
 *     Inf();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Inf
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    struct Valmax;

    /*!
     * \brief Define the tag Inf of functor Inf
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    #ifdef BOOST_SIMD_NO_INFINITIES
    typedef Valmax Inf;
    #else
    struct Inf : ext::pure_constant_<Inf>
    {
      typedef ext::pure_constant_<Inf> parent;
      typedef double default_type;
      template<class Target, class Dummy=void>
      struct apply : meta::make_dependent<Valmax, Dummy>::type::template apply<Target,Dummy> {};
    };

    template<class T, class Dummy>
    struct  Inf::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0x7F800000> {};

    template<class T, class Dummy>
    struct  Inf::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0x7FF0000000000000ULL> {};
    #endif
  }
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Inf, Inf)

} }

#include <boost/simd/toolbox/constant/include/constants/valmax.hpp>
#include <boost/simd/sdk/constant/common.hpp>

#endif


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121213    R.C.A. Boon       Creation of code.
 *      130117    R.C.A. Boon       Added solved boolean flag to applicable member functions (with
 *                                  help from S. Billemont). Moved constructor to header file.
 *                                  Moved debugged getMaximumNumberOfRevolutions to source file.
 *      130211    R.C.A. Boon       Added hasSolution flag to root finder.
 *      120227    S. Billemont      Removed hasSolution in favor of an exception.
 *      130325    R.C.A. Boon       Removed superfluous sanity check of number of revolutions in
 *                                  execute() function, fixed bug in computation of maximumNumberOf-
 *                                  Revolutions.
 *
 *    References
 *      PyKEP toolbox, Dario Izzo, ESA Advanced Concepts Team.
 *      Richard H. An Introduction to the Mathematics and Methods of Astrodynamics, Revised
 *          Edition.
 *      Battin, AIAA Education Series.
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/format.hpp>
#include <boost/math/special_functions.hpp> // for asinh and acosh

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/multiRevolutionLambertTargeterIzzo.h"
#include "Tudat/Mathematics/BasicMathematics/convergenceException.h"

namespace tudat
{
namespace mission_segments
{

//! Compute solution for N revolutions and branch.
void MultiRevolutionLambertTargeterIzzo::computeForRevolutionsAndBranch(
        const int aNumberOfRevolutions, const bool aIsRightBranch )
{
    // Adjust parameters for new solution
    numberOfRevolutions = aNumberOfRevolutions;
    isRightBranch = aIsRightBranch;

    // Check whether number of revolutions is possible
    sanityCheckNumberOfRevolutions( );

    // Execute problem solving for new solution
    execute( );
}

//! Get maximum number of revolutions calculated.
int MultiRevolutionLambertTargeterIzzo::getMaximumNumberOfRevolutions( )
{
    if ( !solved )
    {
        transformDimensions( );
        sanityCheckNumberOfRevolutions( );
    }

    return maximumNumberOfRevolutions;
}

//! Sanity check number of revolutions.
void MultiRevolutionLambertTargeterIzzo::sanityCheckNumberOfRevolutions( )
{
    // If not yet defined, calculate number of revolutions possible.
    if ( maximumNumberOfRevolutions == NO_MAXIMUM_REVOLUTIONS )
    {
        // Temporarily store specified number, as numberOfRevolutions is needed to calculate max
        // (this is a tricky way to work, but on the other hand this makes this approach decidedly
        // different from PyKEP routines and it also happens only once per object).
        int copyOfOriginalNumberOfRevolutions = numberOfRevolutions;

        // Calculate first guess of maximum, by dividing the time of flight of the minimum energy
        // ellipse by the normalized time of flight.
        numberOfRevolutions = static_cast< int >(
                    normalizedTimeOfFlight / (
                        mathematical_constants::PI / 2.0
                        * std::sqrt( 2.0 * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter
                                     * normalizedSemiPerimeter ) ) );

        // If the current guess for the maximum is non-zero, then additional analysis is required to
        // determine the correct maximum.
        if( numberOfRevolutions != 0)
        {
            // The following try-block is meant to check whether the solution converges or not. If
            // the current guess for the maximum number of revolutions is correct, then the problem
            // will converge. If it does not, an exception will be thrown stating that it did not
            // converge. Catching this exception allows to decrease the guess only when the
            // exception occurs, and not under other circumstances.
            try
            {
                // Compute root (no further information is required)
                computeRootTimeOfFlight();
            }
            catch( basic_mathematics::ConvergenceException )
            {
                // If the rootfinder did not converge, then the current guess is wrong and needs to
                // be decreased
                numberOfRevolutions--;
            }
        }
        // No further analysis is needed of the current guess is equal to zero.

        // Maximum is now found.
        maximumNumberOfRevolutions = numberOfRevolutions;

        // Reinstating original number of revolutions specified.
        numberOfRevolutions = copyOfOriginalNumberOfRevolutions;
    }

    // Default: compare maximum with specified number of revolutions.
    // If specified is larger than maximum, no solution is possible.
    if ( numberOfRevolutions > maximumNumberOfRevolutions )
    {
        // Throw exception.
        BOOST_THROW_EXCEPTION( std::runtime_error(
            ( boost::format(
                "Number of revolutions specified in Lambert problem is larger than possible.\n"
                "Specified number of revolutions %d while the maximum is %d"
            ) % numberOfRevolutions % maximumNumberOfRevolutions).str( )
        ) );
    }
    // Else, nothing wrong.
}

//! Execute solving procedure (for multiple revolutions).
void MultiRevolutionLambertTargeterIzzo::execute( )
{
    // Sanity checks.
    sanityCheckTimeOfFlight( );
    sanityCheckGravitationalParameter( );

    // Transform dimensions.
    transformDimensions( );

    /*// Sanity check for number of revolutions (must be after dimension removal).
    sanityCheckNumberOfRevolutions( );*/

    if ( numberOfRevolutions == 0 )
    {
        // call base class function that works on zero revolutions.
        ZeroRevolutionLambertTargeterIzzo::execute( );
    }
    else
    {
        // Solve multi-rev root.
        double multipleRevolutionXParameter = computeRootTimeOfFlight( );

        // Reconstruct velocities.
        computeVelocities( multipleRevolutionXParameter );
    }

    solved = true;
}

//! Compute time-of-flight using Lagrange's equation (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeTimeOfFlight( const double xParameter )
{
    // Determine semi-major axis.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc.
        else
        {
            betaParameter = 2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange including multiple revolutions.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) )
                  + 2.0 * mathematical_constants::PI
                  * numberOfRevolutions );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;

        // If long transfer arc.
        if ( isLongway )
        {
            betaParameter = -2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                    - normalizedChord )
                                                                  / ( -2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * boost::math::asinh( std::sqrt( ( normalizedSemiPerimeter
                                                                   - normalizedChord )
                                                                 / ( -2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlightLagrange = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlightLagrange;
    }
}

//! Solve the time of flight equation for x (for multiple revolutions).
double MultiRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( )
{
    using mathematical_constants::PI;

    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1, x2;

    if ( isRightBranch )
    { // right branch solution.
        x1 = std::tan( .7234 * PI / 2.0 );
        x2 = std::tan( .5234 * PI / 2.0 );
    }
    else
    { // left branch solution.
        x1 = std::tan( -.5234 * PI / 2.0 );
        x2 = std::tan( -.2234 * PI / 2.0 );
    }

    double y1 = computeTimeOfFlight( std::atan( x1 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    double y2 = computeTimeOfFlight( std::atan( x2 ) * 2.0 / PI ) - normalizedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && ( y1 != y2 )
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = computeTimeOfFlight( std::atan( xNew ) * 2.0 / PI ) - normalizedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        BOOST_THROW_EXCEPTION( basic_mathematics::ConvergenceException(
            ( boost::format(
                "Multi-Revolution Lambert targeter failed to converge to a solution.\n"
                "Reached the maximum number of iterations: %d"
            ) % maximumNumberOfIterations).str( )
        ) );
    }

    // Revert to x parameter.
    double xParameter = std::atan( xNew ) * 2.0 / PI;
    return xParameter;
}

// Add compute maximum number of revolutions routine?

} // namespace mission_segments
} // namespace tudat


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

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_RING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_RING_HPP

#include <deque>

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_order.hpp>

#include <boost/geometry/util/order_as_direction.hpp>
#include <boost/geometry/util/range.hpp>

#include <boost/geometry/algorithms/equals.hpp>

#include <boost/geometry/views/closeable_view.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/validity_failure_type.hpp>
#include <boost/geometry/algorithms/detail/num_distinct_consecutive_points.hpp>
#include <boost/geometry/algorithms/detail/is_valid/has_duplicates.hpp>
#include <boost/geometry/algorithms/detail/is_valid/has_invalid_coordinate.hpp>
#include <boost/geometry/algorithms/detail/is_valid/has_spikes.hpp>
#include <boost/geometry/algorithms/detail/is_valid/has_valid_self_turns.hpp>

#include <boost/geometry/strategies/area.hpp>

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#include <boost/geometry/io/dsv/write.hpp>
#endif

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

// struct to check whether a ring is topologically closed
template <typename Ring, closure_selector Closure /* open */>
struct is_topologically_closed
{
    template <typename VisitPolicy>
    static inline bool apply(Ring const&, VisitPolicy& visitor)
    {
        boost::ignore_unused(visitor);

        return visitor.template apply<no_failure>();
    }
};

template <typename Ring>
struct is_topologically_closed<Ring, closed>
{
    template <typename VisitPolicy>
    static inline bool apply(Ring const& ring, VisitPolicy& visitor)
    {
        boost::ignore_unused(visitor);

        if (geometry::equals(range::front(ring), range::back(ring)))
        {
            return visitor.template apply<no_failure>();
        }
        else
        {
            return visitor.template apply<failure_not_closed>();
        }
    }
};

template <typename ResultType, bool IsInteriorRing /* false */>
struct ring_area_predicate
{
    typedef std::greater<ResultType> type;
};

template <typename ResultType>
struct ring_area_predicate<ResultType, true>
{
    typedef std::less<ResultType> type;
};

template <typename Ring, bool IsInteriorRing>
struct is_properly_oriented
{
    typedef typename point_type<Ring>::type point_type;

    typedef typename strategy::area::services::default_strategy
        <
            typename cs_tag<point_type>::type,
            point_type
        >::type strategy_type;

    typedef detail::area::ring_area
        <
            order_as_direction<geometry::point_order<Ring>::value>::value,
            geometry::closure<Ring>::value
        > ring_area_type;

    typedef typename default_area_result<Ring>::type area_result_type;

    template <typename VisitPolicy>
    static inline bool apply(Ring const& ring, VisitPolicy& visitor)
    {
        boost::ignore_unused(visitor);

        typename ring_area_predicate
            <
                area_result_type, IsInteriorRing
            >::type predicate;

        // Check area
        area_result_type const zero = area_result_type();
        if (predicate(ring_area_type::apply(ring, strategy_type()), zero))
        {
            return visitor.template apply<no_failure>();
        }
        else
        {
            return visitor.template apply<failure_wrong_orientation>();
        }
    }
};

template
<
    typename Ring,
    bool CheckSelfIntersections = true,
    bool IsInteriorRing = false
>
struct is_valid_ring
{
    template <typename VisitPolicy>
    static inline bool apply(Ring const& ring, VisitPolicy& visitor)
    {
        // return invalid if any of the following condition holds:
        // (a) the ring's point coordinates are not invalid (e.g., NaN)
        // (b) the ring's size is below the minimal one
        // (c) the ring consists of at most two distinct points
        // (d) the ring is not topologically closed
        // (e) the ring has spikes
        // (f) the ring has duplicate points (if AllowDuplicates is false)
        // (g) the boundary of the ring has self-intersections
        // (h) the order of the points is inconsistent with the defined order
        //
        // Note: no need to check if the area is zero. If this is the
        // case, then the ring must have at least two spikes, which is
        // checked by condition (d).

        if (has_invalid_coordinate<Ring>::apply(ring, visitor))
        {
            return false;
        }

        closure_selector const closure = geometry::closure<Ring>::value;
        typedef typename closeable_view<Ring const, closure>::type view_type;

        if (boost::size(ring)
            < core_detail::closure::minimum_ring_size<closure>::value)
        {
            return visitor.template apply<failure_few_points>();
        }

        view_type const view(ring);
        if (detail::num_distinct_consecutive_points
                <
                    view_type, 4u, true,
                    not_equal_to<typename point_type<Ring>::type>
                >::apply(view)
            < 4u)
        {
            return
                visitor.template apply<failure_wrong_topological_dimension>();
        }

        return
            is_topologically_closed<Ring, closure>::apply(ring, visitor)
            && ! has_duplicates<Ring, closure>::apply(ring, visitor)
            && ! has_spikes<Ring, closure>::apply(ring, visitor)
            && (! CheckSelfIntersections
                || has_valid_self_turns<Ring>::apply(ring, visitor))
            && is_properly_oriented<Ring, IsInteriorRing>::apply(ring, visitor);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// A Ring is a Polygon with exterior boundary only.
// The Ring's boundary must be a LinearRing (see OGC 06-103-r4,
// 6.1.7.1, for the definition of LinearRing)
//
// Reference (for polygon validity): OGC 06-103r4 (6.1.11.1)
template <typename Ring, bool AllowEmptyMultiGeometries>
struct is_valid
    <
        Ring, ring_tag, AllowEmptyMultiGeometries
    > : detail::is_valid::is_valid_ring<Ring>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_RING_HPP