#include "dvs_depth_from_defocus/depth_defocus_node.hpp"

#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>

//#define DEBUG_SHOW_FAKE_FRAMES
// #define DEBUG_SHOW_FILTERED_DEPTHMAP

#include <cv_bridge/cv_bridge.h>
#include <dvs_slam_msgs/VoxelGrid.h>
#include <minkindr_conversions/kindr_tf.h>
#include <tf/transform_listener.h>

#include <Eigen/Geometry>
#include <opencv2/highgui/highgui.hpp>
#ifdef MAPPING_PERF
#include "evo_utils/main.hpp"
#endif

#include "rpg_common_ros/params_helper.hpp"

namespace depth_from_defocus {

using namespace evo_utils::geometry;
using namespace evo_utils::camera;

// 只读一次，相机信息处理部分
void DepthFromDefocusNode::cameraInfoCallback(
    const sensor_msgs::CameraInfo::ConstPtr& msg) {
    static bool got_camera_info = false;

    if (!got_camera_info) {
        dvs_cam_.fromCameraInfo(*msg);

        postCameraLoaded();

        // Currently, done only once
        got_camera_info = true;
    }
}

void DepthFromDefocusNode::postCameraLoaded() {
    cv::Size full_resolution = dvs_cam_.fullResolution();
    width_ = full_resolution.width;
    height_ = full_resolution.height;
    // 虚拟相机的参数
    const float fov_virtual_cam_deg = rpg_common_ros::param<float>(nh_, "fov_virtual_camera_deg", 0.0);
    virtual_width_ = rpg_common_ros::param<int>(nh_, "virtual_width", width_);
    virtual_height_ = rpg_common_ros::param<int>(nh_, "virtual_height", height_);
    //
    float f_virtual_cam_;
    if (fov_virtual_cam_deg == 0.) {
        f_virtual_cam_ = dvs_cam_.fx();
    } else {
        const float fov_virtual_cam_rad = fov_virtual_cam_deg * CV_PI / 180.0;
        f_virtual_cam_ =
            0.5 * (float)virtual_width_ / std::tan(0.5 * fov_virtual_cam_rad);
    }
    LOG(INFO) << "Focal length of virtual camera: " << f_virtual_cam_
              << " pixels";

    virtual_cam_ = PinholeCamera(
        virtual_width_, virtual_height_, f_virtual_cam_, f_virtual_cam_,
        0.5 * (float)virtual_width_, 0.5 * (float)virtual_height_);
    // 原本相机的内参
    K_ << dvs_cam_.fx(), 0.f, dvs_cam_.cx(), 0.f, dvs_cam_.fy(), dvs_cam_.cy(),
        0.f, 0.f, 1.f;

    precomputeRectifiedPoints();
    resetMapper();
}

// precomputed_rectified_points_ 用 dvs_cam_ 矫正
void DepthFromDefocusNode::precomputeRectifiedPoints() {
    precomputed_rectified_points_ = Eigen::Matrix2Xf(2, height_ * width_);
    for (int y = 0; y < height_; y++) {
        for (int x = 0; x < width_; ++x) {
            cv::Point2d rectified_point =
                dvs_cam_.rectifyPoint(cv::Point2d(x, y));
            precomputed_rectified_points_.col(y * width_ + x) =
                Eigen::Vector2f(rectified_point.x, rectified_point.y);
        }
    }
}

DepthFromDefocusNode::DepthFromDefocusNode(
    const ros::NodeHandle& nh, const ros::NodeHandle& pnh,
    const image_geometry::PinholeCameraModel& cam)
    : nh_(nh),
      pnh_(pnh),
      it_(nh),
      state_(IDLE),
      dvs_cam_(cam),
      pc_(new PointCloud),
      pc_global_(new PointCloud) {

    radius_search_ = rpg_common_ros::param<float>(pnh_, "radius_search", 0.05);
    min_num_neighbors_ =
        rpg_common_ros::param<int>(pnh_, "min_num_neighbors", 3);
    median_filter_size_ =
        rpg_common_ros::param<int>(pnh_, "median_filter_size", 7);

    regular_frame_id_ =
        rpg_common_ros::param(nh_, "dvs_frame_id", std::string("dvs"));
    bootstrap_frame_id_ =
        rpg_common_ros::param(nh_, "dvs_bootstrap_frame_id", regular_frame_id_);
    frame_id_ = bootstrap_frame_id_;
    world_frame_id_ =
        rpg_common_ros::param<std::string>(nh_, "world_frame_id", "world");

    tf_ = std::make_shared<tf::Transformer>(true, ros::Duration(10000.0));

    tf_sub_ = nh_.subscribe("tf", 0, &DepthFromDefocusNode::tfCallback, this);
    event_sub_ = nh_.subscribe("events", 0,
                               &DepthFromDefocusNode::processEventArray, this);
    remote_key_ = nh_.subscribe("remote_key", 0,
                                &DepthFromDefocusNode::remoteKeyCallback, this);
    copilot_sub_ = nh_.subscribe("/evo/copilot_remote", 0,
                                 &DepthFromDefocusNode::copilotCallback, this);
    // PointCloud
    pub_pc_ = pnh_.advertise<PointCloud>("pointcloud", 1);
    pc_->header.frame_id = world_frame_id_;
    // PointCloud
    pub_pc_global_ = pnh_.advertise<PointCloud>("pointcloud_global", 1);
    pc_global_->header.frame_id = world_frame_id_;
    // dvs_slam_msgs::VoxelGrid
    pub_voxel_grid_ = pnh_.advertise<dvs_slam_msgs::VoxelGrid>("voxel_grid", 1);
    // events_to_recreate_kf_
    events_to_recreate_kf_ =
        rpg_common_ros::param<int>(pnh_, "events_to_recreate_kf", 2000000);

    camera_info_sub_ = nh_.subscribe(
        "camera_info", 1, &DepthFromDefocusNode::cameraInfoCallback, this);
    // 同样两次
    postCameraLoaded();

    auto_trigger_ = rpg_common_ros::param<bool>(pnh_, "auto_trigger", false);
    if (auto_trigger_) state_ = MAPPING;
}

// callback
void DepthFromDefocusNode::copilotCallback(const std_msgs::Bool& msg) {
    frame_id_ = (msg.data) ? regular_frame_id_ : bootstrap_frame_id_;
    LOG(INFO) << "Mapping switched copilot to " << frame_id_;
}

void DepthFromDefocusNode::tfCallback(const tf::tfMessage::ConstPtr& tf_msg) {
    // 空闲时不订阅
    if (state_ == IDLE /* && !auto_trigger_*/) {
        return;
    }

    for (const geometry_msgs::TransformStamped& transform_stamped_msg :
         tf_msg->transforms) {
        tf::StampedTransform t;
        // geometry_msgs::TransformStamped ->  tf::StampedTransform
        tf::transformStampedMsgToTF(transform_stamped_msg, t);
        // Add transform information to the tf data structure.
        tf_->setTransform(t);

        // LOG(INFO) << transform_stamped_msg.child_frame_id << " " << frame_id_
        //           << " " << world_frame_id_ << " "
        //           << transform_stamped_msg.header.frame_id;

        if (transform_stamped_msg.child_frame_id == frame_id_) {
            // if (state_ == IDLE && auto_trigger_) {
            //     state_ = MAPPING;
            //     return;  // there are no events collected yet
            // }
            // 找到 接近当前位姿的下标
            while (newest_tracked_event_ < event_queue_.size() &&
                   event_queue_[newest_tracked_event_ + 1].ts <
                       transform_stamped_msg.header.stamp)
                ++newest_tracked_event_;

            // LOG(INFO) << newest_tracked_event_ << " " <<
            // events_to_recreate_kf_
            //           << " " << auto_trigger_;

            if (frame_id_ == bootstrap_frame_id_) {
                // keep bootstrap frame also as regular frame for future use
                t.child_frame_id_ = regular_frame_id_;
                tf_->setTransform(t);
            }

            if (auto_trigger_ && (newest_tracked_event_ - current_event_ >
                                  events_to_recreate_kf_)) {
                // 更新发送
                update();
                auto_trigger_ = false;
                frame_id_ = regular_frame_id_;
            }
        }
    }
}

void DepthFromDefocusNode::remoteKeyCallback(
    const std_msgs::String::ConstPtr& msg) {
    std::string command_str = msg->data;
    LOG(INFO) << "Received command: " << command_str;

    switch (state_) {
        case MAPPING:
            if (command_str == "update") {
#ifdef MAPPING_PERF
                TIMER_START(t1);
#endif
                update();
#ifdef MAPPING_PERF
                TIMER_STOP(t1, t2, duration);
                LOG(INFO) << "[Map update] comp. time: " << duration
                          << " milliseconds";
#endif
            } else if (command_str == "reset") {
                LOG(INFO) << "Resetting mapper. Will continue to respond to "
                             "'update' requests";
                resetMapper();
            } else if (command_str == "disable_map_expansion") {
                LOG(INFO) << "Switching mapper to IDLE mode. Will stop "
                             "treating 'update' requests";
                resetMapper();
                //
                state_ = IDLE;
            }
            break;

        case IDLE:
        default:
            if (command_str == "reset" ||
                command_str == "enable_map_expansion" ||
                command_str == "bootstrap") {
                state_ = MAPPING;
                if (command_str == "bootstrap") auto_trigger_ = true;
                LOG(INFO) << "Swiching to MAPPING mode";
            } else if (command_str == "update" || command_str == "switch") {
                LOG(INFO) << "Ignoring command '" << command_str
                          << "' because I am in IDLE mode";
            }
            break;
    }
}

void DepthFromDefocusNode::processEventArray(
    const dvs_msgs::EventArray::ConstPtr& event_array) {
    if (state_ == IDLE) {
        return;
    }

    for (const dvs_msgs::Event& e : event_array->events)
        event_queue_.push_back(e);

    // Add initial pose
    static bool sent_initial_pose = false;
    if (!sent_initial_pose) {
        tf::StampedTransform T(tf::Transform::getIdentity(), event_queue_[0].ts,
                               world_frame_id_, frame_id_);
        tf_->setTransform(T);
        sent_initial_pose = true;
    }
}

//////////////////      UPDATE  ////////////////////

void DepthFromDefocusNode::update() {
    resetVoxelGrid();
    processEventQueue(true);
    synthesizeAndPublishDepth();
    publishGlobalMap();

    last_kf_update_event_ = current_event_;
    clearEventQueue();
}

void DepthFromDefocusNode::resetMapper() {
    event_queue_.clear();
    tf_->clear();
    pc_->clear();
    pc_global_->clear();
    setupVoxelGrid();
    current_event_ = newest_tracked_event_ = last_kf_update_event_ = 0;
    T_ref_w_.setIdentity();
}

 void DepthFromDefocusNode::processEventQueue(bool reset) {
    static const size_t skip_batches_normal =
                            rpg_common_ros::param<int>(pnh_, "skip_batches", 0),
                        skip_batches_reset = rpg_common_ros::param<int>(
                            pnh_, "skip_batches_for_reset", 0),
                        max_event_rate = rpg_common_ros::param<int>(
                            pnh_, "max_event_rate", 10000000),
                        frame_size = rpg_common_ros::param<int>(
                            pnh_, "frame_size", 1024),
                        min_batch_size = rpg_common_ros::param<int>(
                            pnh_, "min_batch_size", 20000);
    // frame_size	number of events to aggregate when computing new ma
    // min_batch_size	minimum number of new events required for a map update
    // skip_batches	amount of batches of events to skip during optimization
    // skip_batches_for_reset	amount of batches of events to skip during optimization after a reset

    // Performance measurement
    static const size_t perf_interval = 1000000;
    static size_t events_processed = 0;
    static double last_ts = event_queue_[current_event_].ts.toSec(),
                  time_evolved = 0;
    // size_t current_event_;         next event to process
    // size_t newest_tracked_event_;  first event for which the position is unknown
    // 将要处理的事件数 current_event_ ~ newest_tracked_event_
    const size_t n_events = newest_tracked_event_ - current_event_;
    //
    if (n_events < min_batch_size) return;
    // 根据事件速率 再调整 优化时 要跳的数目
    size_t skip_batches = (reset) ? skip_batches_reset : skip_batches_normal;
    // 不reset时 ，Adapt skip_batches to max_event_rate
    const double duration = (event_queue_[newest_tracked_event_].ts -
                             event_queue_[current_event_].ts)
                                .toSec();
    const float event_rate = n_events / duration;
    if (!reset && event_rate > max_event_rate) {
        skip_batches = ceil(event_rate / max_event_rate) - 1;
        //    LOG(WARNING) << "Adapt skip_batches to: " << skip_batches;
    }

    static std::vector<Eigen::Vector4f> events;
    static std::vector<Eigen::Vector3f> centers;
    events.clear();
    centers.clear();

#ifdef MAPPING_PERF
    TIMER_START(t1);
#endif

    // Group events by packets events
    long num_events_processed = 0;

#ifdef DEBUG_SHOW_FAKE_FRAMES
    cv::Mat integrated_img = cv::Mat::zeros(height_, width_, CV_8U);
#endif

    while (current_event_ + (skip_batches + 1) * frame_size <
           newest_tracked_event_) {

        evo_utils::geometry::Transformation T_w_cur, T_ref_cur;
        // 线性插值 得出 current_event_ + frame_size / 2 位置的 位姿
        getPoseAt(event_queue_[current_event_ + frame_size / 2].ts, T_w_cur);
        // T_ref_w_
        T_ref_cur = T_ref_w_ * T_w_cur;
        const evo_utils::geometry::Transformation T_cur_ref =
            T_ref_cur.inverse();

        const Eigen::Matrix3f R = T_cur_ref.getRotationMatrix().cast<float>();
        const Eigen::Vector3f t = T_cur_ref.getPosition().cast<float>();

        // Project the points on plane at distance z0
        // Planar homography  (H_z0)^-1 that maps a point in the reference view to the event camera through plane Z = Z0 (Eq. (8) in the IJCV paper)
        // 比论文多乘z0 这样映射到 reference view 的 z0 平面 而不是 1 平面
        // 注意：Hz0 实际上是相机坐标系下的变化
        //       Hz0 inv ：reference view to the event camera

        const float z0 = raw_depths_vec_[0];
        Eigen::Matrix3f R_corr = R;
        R_corr *= z0;
        R_corr.col(2) += t;
        // res坐标系下的位置
        centers.push_back(-R.transpose() * t);
        // DVS camera   ： K_
        // virtual_cam_ ： Kinv_ （由dvs视距决定）
        Eigen::Matrix3f H_cur_ref = K_ * R_corr * virtual_cam_.Kinv_;
        Eigen::Matrix3f H_ref_cur = H_cur_ref.inverse();

        Eigen::Matrix4f H_ref_cur_4x4;
        H_ref_cur_4x4.block<3, 3>(0, 0) = H_ref_cur;
        H_ref_cur_4x4.col(3).setZero();
        H_ref_cur_4x4.row(3).setZero();
        // For each frame_size, precompute the warped event locations according to Eq. (11) in the IJCV paper.
        for (size_t i = 0; i != frame_size; ++i) {
            const dvs_msgs::Event& e = event_queue_[current_event_];
            Eigen::Vector4f p;
            // 用 dvs 内参矫正的
            p.head<2>() = precomputed_rectified_points_.col(e.y * width_ + e.x);
            p[2] = 1.;
            p[3] = 0.;
            p = H_ref_cur_4x4 * p;
            p /= p[2];
            p[2] = 1.;
            events.push_back(p);
            ++num_events_processed;
            ++current_event_;
#ifdef DEBUG_SHOW_FAKE_FRAMES
            integrated_img.at<uchar>(e.y, e.x) = 255;
#endif
        }
        // 更新current_event_，使用了第一个frame_size 跳过了skip_batches个
        current_event_ += skip_batches * frame_size;

#ifdef DEBUG_SHOW_FAKE_FRAMES
        cv::imshow("Integrated events", integrated_img);
        cv::waitKey(10);
        integrated_img = cv::Mat::zeros(height_, width_, CV_8U);
#endif
    }// end while

    // 还有这种情况
     if (centers.size() == 0) return;
    //
    projectEventsToVoxelGrid(events, centers);

#ifdef MAPPING_PERF
    events_processed += n_events;
    {
        TIMER_STOP(t1, t2, duration);
        time_evolved += duration / 1000;  // from ms to s
    }

    if (events_processed > perf_interval) {
        double cur_ts = event_queue_[current_event_].ts.toSec(),
               time_processed = cur_ts - last_ts;
        LOG(INFO) << "  Events/s: " << (double)events_processed / time_evolved;
        LOG(INFO) << "Event rate: "
                  << (double)events_processed / time_processed;
        LOG(WARNING) << " RT factor: " << time_processed / time_evolved;

        last_ts = cur_ts;
        time_evolved = 0;
        events_processed = 0;
    }
#endif
}

// It maps events from plane Z0 to all the planes Zi of the DSI using Eq. (15)
// and then votes for the corresponding voxel using bilinear voting.
void DepthFromDefocusNode::projectEventsToVoxelGrid(
    const std::vector<Eigen::Vector4f>& events,
    const std::vector<Eigen::Vector3f>& centers) {

     // For efficiency reasons, we split each packet into batches of N events each //
    // which allows to better exploit the L1 cache
    static const int N = 128;
    typedef Eigen::Array<float, N, 1> Arrayf;

    const size_t frame_size = events.size() / centers.size();
    const float z0 = raw_depths_vec_[0];

    // 一个相机 一个 frame_size = N * batch 个事件
#pragma omp parallel for if (events.size() >= 20000)
    // 不同深度平面的循环
    for (size_t layer = 0; layer < raw_depths_vec_.size(); ++layer) {
        // 每一层上，遍历所有事件
        const Eigen::Vector4f* pe = &events[0];
        // ref_voxel_grid_ 存有所有
        Vote* pgrid = &ref_voxel_grid_[layer * virtual_width_ * virtual_height_];
        //
        for (size_t frame = 0; frame != centers.size(); ++frame) {

            const Eigen::Vector3f& C = centers[frame];
            const float zi = (float)raw_depths_vec_[layer],
                        a = z0 * (zi - C[2]),
                        bx = (z0 - zi) * (C[0] * virtual_cam_.fx_ +
                                          C[2] * virtual_cam_.cx_),
                        by = (z0 - zi) * (C[1] * virtual_cam_.fy_ +
                                          C[2] * virtual_cam_.cy_),
                        d = zi * (z0 - C[2]);

            // Update voxel grid now, N events per iteration
            // 一个相机上有frame_size个，现在又分
            for (size_t batch = 0; batch != frame_size / N; ++batch) {
                Arrayf X, Y;
                for (size_t i = 0; i != N; ++i) {
                    X[i] = pe[i][0];
                    Y[i] = pe[i][1];
                }
                X = (X * a + bx) / d;
                Y = (Y * a + by) / d;
                // pgrid 为 深度平面上某个点
                for (size_t i = 0; i != N; ++i)
                    voteForCellBilinear(X[i], Y[i], pgrid);
                // 滑过 N 个 events
                pe += N;
            }
        }
    }
}

void DepthFromDefocusNode::resetVoxelGrid() {
    // 重置体素，意味着需要更新地图
    std::fill(ref_voxel_grid_.begin(), ref_voxel_grid_.end(), 0);

    evo_utils::geometry::Transformation T_w_cur;
    ros::Time last_stamp = event_queue_[newest_tracked_event_].ts;
    // 初始 next event to process
    current_event_ = (newest_tracked_event_ > events_to_recreate_kf_)
                         ? newest_tracked_event_ - events_to_recreate_kf_
                         : 0u;

    // Center the voxel grid in the middle pose of the sequence of events
    // used to fill the DSI.
    getPoseAt(last_stamp, T_w_cur);

    LOG(INFO) << "Received a map creation request";
    LOG(INFO) << "Setting KF at time: " << last_stamp;
    //
    T_ref_w_ = T_w_cur.inverse();
}

DepthFromDefocusNode::~DepthFromDefocusNode() {
    synthesizeAndPublishDepth();
    publishGlobalMap();
}

// 初始化逆深度向量和体素
void DepthFromDefocusNode::setupVoxelGrid() {
    evo_utils::geometry::Depth min_depth =
        rpg_common_ros::param<float>(nh_, "min_depth", 1.0);
    evo_utils::geometry::Depth max_depth =
        rpg_common_ros::param<float>(nh_, "max_depth", 10.0);
    num_depth_cells_ =
        (size_t)rpg_common_ros::param<int>(nh_, "num_depth_cells", 64);

    adaptive_threshold_kernel_size_ =
        rpg_common_ros::param<int>(pnh_, "adaptive_threshold_kernel_size", 5);

    adaptive_threshold_c_ =
        rpg_common_ros::param<int>(pnh_, "adaptive_threshold_c", -10);

    CHECK_GT(adaptive_threshold_kernel_size_, 0);
    CHECK_EQ(adaptive_threshold_kernel_size_ % 2, 1);

    CHECK_GT(min_depth, 0.0);
    CHECK_GT(max_depth, min_depth);

    CHECK_LE(num_depth_cells_, 256u) << "Number of depth cells must be <= 256";

    //  depths_vec_ = LinearDepthVector(min_depth, max_depth, num_depth_cells_);
    depths_vec_ = InverseDepthVector(min_depth, max_depth, num_depth_cells_);
    raw_depths_vec_ = depths_vec_.getDepthVector();

    min_depth_ = min_depth;
    max_depth_ = max_depth;

    ref_voxel_grid_.resize(num_depth_cells_ * virtual_width_ * virtual_height_,
                           0);
}

void DepthFromDefocusNode::synthesizeAndPublishDepth() {
    //  LOG(INFO) << "Synthesize depth from voxel grid";
    cv::Mat depth, confidence;
    synthesizePointCloudFromVoxelGrid(depth, confidence);

    // Accumulate point cloud
    const evo_utils::geometry::Transformation T_w_ref = T_ref_w_.inverse();
    accumulatePointcloud(depth, confidence_mask_, confidence,
                         T_w_ref.getRotation().getRotationMatrix(),
                         T_w_ref.getPosition(),
                         event_queue_[current_event_].ts);

    publishVoxelGrid(T_w_ref);
    publishDepthmap(depth, confidence_mask_);
}

/* // Generic way to try different focus measures, but slow.
void DepthFromDefocusNode::synthesizePointCloudFromVoxelGridContrast(cv::Mat&
depth_cell_indices, cv::Mat &confidence)
{
  // Bell-shaped mask for a patch
  const int half_patchsize = rpg_common_ros::param<int>(pnh_, "half_patchsize",
3); const int patch_size = 2 * half_patchsize + 1; cv::Mat gaussian_kernel =
cv::getGaussianKernel(patch_size, -1, CV_32F); cv::Mat gaussian_kernel_2d =
gaussian_kernel * gaussian_kernel.t();

  for(int y=half_patchsize; y<virtual_height_-half_patchsize; ++y)
  {
    for(int x=half_patchsize; x<virtual_width_-half_patchsize; ++x)
    {
      // Build vector containing the number of votes as a function of depth
      for(size_t depth_plane=0; depth_plane<num_depth_cells_; ++depth_plane)
      {
        Vote *pgrid = &ref_voxel_grid_[depth_plane * virtual_width_ *
virtual_height_]; cv::Mat slice_dsi(virtual_height_, virtual_width_, CV_32F,
pgrid);

        // Compute the contrast on a small patch around (x,y)
        cv::Scalar mean, stddev;
        cv::Mat patch = slice_dsi(cv::Rect(x-half_patchsize, y-half_patchsize,
patch_size, patch_size)).mul(gaussian_kernel_2d); cv::meanStdDev(patch, mean,
stddev);

        // Select maximum contrast along the optical ray
        if(stddev[0] > confidence.at<float>(y,x))
        {
          confidence.at<float>(y,x) = (float) stddev[0]; // max of contrast
along optical ray depth_cell_indices.at<uchar>(y,x) = depth_plane;
        }
      }
    }
  }
}
*/

void DepthFromDefocusNode::synthesizePointCloudFromVoxelGridContrast(
    cv::Mat& depth_cell_indices, cv::Mat& confidence) {
    // Bell-shaped mask for a patch
    const int half_patchsize =
        rpg_common_ros::param<int>(pnh_, "half_patchsize", 3);
    const int patch_size = 2 * half_patchsize + 1;

    // Local mean, mean square and standard deviation of a depth slice of the
    // DSI
    cv::Mat slice_local_mean, slice_local_MS, slice_local_var;

    for (size_t depth_plane = 0; depth_plane < num_depth_cells_;
         ++depth_plane) {
        // Get depth plane
        Vote* pgrid =
            &ref_voxel_grid_[depth_plane * virtual_width_ * virtual_height_];
        cv::Mat slice_dsi(virtual_height_, virtual_width_, CV_32F, pgrid);

        // Compute local variance (contrast) of current depth slice of the DSI
        cv::GaussianBlur(slice_dsi, slice_local_mean,
                         cv::Size(patch_size, patch_size), 0, 0);
        cv::GaussianBlur(slice_dsi.mul(slice_dsi), slice_local_MS,
                         cv::Size(patch_size, patch_size), 0, 0);
        slice_local_var =
            slice_local_MS - slice_local_mean.mul(slice_local_mean);

        // Retain maximum local variance (contrast) per pixel (i.e., per optical
        // ray)
        for (int y = half_patchsize; y < virtual_height_ - half_patchsize;
             ++y) {
            for (int x = half_patchsize; x < virtual_width_ - half_patchsize;
                 ++x) {
                // Compute the contrast on a small patch around (x,y)
                float contrast = slice_local_var.at<float>(y, x);

                // Select maximum contrast along the optical ray
                if (contrast > confidence.at<float>(y, x)) {
                    confidence.at<float>(y, x) =
                        contrast;  // max of contrast along optical ray
                    depth_cell_indices.at<uchar>(y, x) = depth_plane;
                }
            }
        }
    }

    // Return standard deviation instead of variance
    cv::sqrt(confidence, confidence);
}

void DepthFromDefocusNode::synthesizePointCloudFromVoxelGridGradMag(
    cv::Mat& depth_cell_indices, cv::Mat& confidence) {
    // Bell-shaped mask for a patch
    const int half_patchsize =
        rpg_common_ros::param<int>(pnh_, "half_patchsize", 3);
    const int patch_size = 2 * half_patchsize + 1;

    // Gradient of a depth slice of the DSI
    cv::Mat grad_x, grad_y, grad_mag;

    for (size_t depth_plane = 0; depth_plane < num_depth_cells_;
         ++depth_plane) {
        // Get depth plane
        Vote* pgrid =
            &ref_voxel_grid_[depth_plane * virtual_width_ * virtual_height_];
        cv::Mat slice_dsi(virtual_height_, virtual_width_, CV_32F, pgrid);

        // Compute local gradient magnitude (focus measure) of current depth
        // slice of the DSI
        cv::Sobel(slice_dsi, grad_x, CV_32F, 1, 0, patch_size);
        cv::Sobel(slice_dsi, grad_y, CV_32F, 0, 1, patch_size);
        grad_mag = grad_x.mul(grad_x) + grad_y.mul(grad_y);

        // Retain maximum local focus per pixel (i.e., per optical ray)
        for (int y = half_patchsize; y < virtual_height_ - half_patchsize;
             ++y) {
            for (int x = half_patchsize; x < virtual_width_ - half_patchsize;
                 ++x) {
                // Compute the focus measure on a small patch around (x,y)
                float focus = grad_mag.at<float>(y, x);

                // Select maximum focus value along the optical ray
                if (focus > confidence.at<float>(y, x)) {
                    confidence.at<float>(y, x) =
                        focus;  // max focus along optical ray
                    depth_cell_indices.at<uchar>(y, x) = depth_plane;
                }
            }
        }
    }

    // Return gradient magnitude instead of its square
    cv::sqrt(confidence, confidence);
}

void DepthFromDefocusNode::synthesizePointCloudFromVoxelGridLinf(
    cv::Mat& depth_cell_indices, cv::Mat& confidence) {
    // Storage for 1-D slice of the DSI (along optical ray), i.e.,
    // vector containing the number of votes as a function of depth
    std::vector<float> num_votes_vec(num_depth_cells_);

    for (int y = 0; y < virtual_height_; ++y) {
        for (int x = 0; x < virtual_width_; ++x) {
            // Build vector containing the number of votes as a function of
            // depth
            for (size_t depth_plane = 0; depth_plane < num_depth_cells_;
                 ++depth_plane) {
                num_votes_vec.at(depth_plane) =
                    voxelGridAt(ref_voxel_grid_, x, y, depth_plane);
            }

            // Look for inv_depth* = argmax(sharpness(inv_depth))
            auto max = std::max_element(std::begin(num_votes_vec),
                                        std::end(num_votes_vec));

            confidence.at<float>(y, x) =
                *max;  // max number of votes along optical ray
            depth_cell_indices.at<uchar>(y, x) =
                std::distance(std::begin(num_votes_vec), max);
        }
    }
}

void DepthFromDefocusNode::synthesizePointCloudFromVoxelGrid(
    cv::Mat& depth, cv::Mat& confidence) {
    confidence = cv::Mat::zeros(virtual_height_, virtual_width_, CV_32F);
    cv::Mat depth_cell_indices =
        cv::Mat::zeros(virtual_height_, virtual_width_, CV_8U);

    // Choose method to obtain depth and confidence maps from the DSI
    const int type_focus_measure =
        rpg_common_ros::param<int>(pnh_, "type_focus_measure", 0);

#ifdef MAPPING_PERF
    TIMER_START(t1);
#endif

    if (type_focus_measure == 1) {
        // LOG(INFO) << "Depth estimation by maximum local variance of DSI";
        // Maximum patch contrast (variance) along optical ray
        synthesizePointCloudFromVoxelGridContrast(depth_cell_indices,
                                                  confidence);
    } else if (type_focus_measure == 2) {
        // LOG(INFO) << "Depth estimation by maximum local gradient magnitude of
        // DSI"; Maximum patch focus along optical ray
        synthesizePointCloudFromVoxelGridGradMag(depth_cell_indices,
                                                 confidence);
    } else {
        // LOG(INFO) << "Depth estimation by maximum of DSI";
        // Maximum number of votes along optical ray
        synthesizePointCloudFromVoxelGridLinf(depth_cell_indices, confidence);
    }

#ifdef MAPPING_PERF
    TIMER_STOP(t1, t2, duration);
    LOG(INFO) << "[Focus measure] comp. time: " << duration << " milliseconds";
#endif

    // Adaptive thresholding to get the confidence map
    cv::Mat confidence_map;
    cv::normalize(confidence, confidence_map, 0.0, 255.0, cv::NORM_MINMAX);
    confidence_map.convertTo(confidence_map, CV_8U);

    cv::adaptiveThreshold(confidence_map, confidence_mask_, 1,
                          cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY,
                          adaptive_threshold_kernel_size_,
                          -adaptive_threshold_c_);

    //  LOG(INFO) << "Median filtering depth map...";
    cv::Mat depth_cell_indices_filtered;
    // rpg_common::Timer median_filtering_timer;
    // median_filtering_timer.start();
    evo_utils::geometry::huangMedianFilter(
        depth_cell_indices, depth_cell_indices_filtered, confidence_mask_,
        median_filter_size_);
    //  LOG(INFO) << "Done in " << median_filtering_timer.stop() << " s";

    depth = cv::Mat(virtual_height_, virtual_width_, CV_32F,
                    cv::Scalar(max_depth_));
    for (int y = 0; y < virtual_height_; ++y) {
        for (int x = 0; x < virtual_width_; ++x) {
            depth.at<float>(y, x) = depths_vec_.cellIndexToDepth(
                depth_cell_indices_filtered.at<uchar>(y, x));
        }
    }

#ifdef DEBUG_SHOW_FILTERED_DEPTHMAP
    cv::Mat norm_depth;
    cv::normalize(depth, norm_depth, 0.f, 1.f, cv::NORM_MINMAX);
    cv::imshow("Raw depthmap", norm_depth);

    // cv::Mat norm_filtered_depth;
    // cv::normalize(filtered_depth, norm_filtered_depth, 0.f, 1.f,
    //               cv::NORM_MINMAX);
    // cv::imshow("Filtered depthmap", norm_filtered_depth);
    cv::waitKey(0);
#endif
}

void DepthFromDefocusNode::accumulatePointcloud(
    const cv::Mat& depth, const cv::Mat& mask, const cv::Mat& confidence,
    const Eigen::Matrix3d R_world_ref, const Eigen::Vector3d t_world_ref,
    const ros::Time& timestamp) {
    pc_->clear();

    for (int y = 0; y < virtual_height_; ++y) {
        for (int x = 0; x < virtual_width_; ++x) {
            if (mask.at<uint8_t>(y, x) > 0) {
                BearingVector f_ref =
                    virtual_cam_.projectPixelTo3dRay(Keypoint(x, y));
                f_ref.normalize();
                Eigen::Vector3d xyz_world =
                    R_world_ref * (f_ref / f_ref[2] * depth.at<float>(y, x)) +
                    t_world_ref;

                pcl::PointXYZI p_world;
                p_world.x = xyz_world.x();
                p_world.y = xyz_world.y();
                p_world.z = xyz_world.z();
                p_world.intensity = 1.0 / p_world.z;
                pc_->push_back(p_world);
            }
        }
    }

    // filter point cloud to remove outliers
    PointCloud::Ptr cloud_filtered(new PointCloud);
    pcl::RadiusOutlierRemoval<PointType> outrem;
    outrem.setInputCloud(pc_);
    outrem.setRadiusSearch(radius_search_);
    outrem.setMinNeighborsInRadius(min_num_neighbors_);
    outrem.filter(*cloud_filtered);

    pc_->swap(*cloud_filtered);
    std_msgs::Header ros_header = pcl_conversions::fromPCL(pc_->header);
    ros_header.stamp = timestamp;
    pcl_conversions::toPCL(ros_header, pc_->header);

    sensor_msgs::PointCloud2::Ptr pc_to_publish(new sensor_msgs::PointCloud2);

    if (!pc_->empty()) {
        pcl::toROSMsg(*pc_, *pc_to_publish);
        pc_to_publish->header.stamp = timestamp;
        pub_pc_.publish(pc_to_publish);
        //    std::cout << "Publish pointcloud (" << pc_->size() << " points)"
        //    << std::endl;
    }

    // Print statistics
    //  Vote max = 0;
    //  int count = 0;
    //  float avg = 0;
    //  for (const Vote &v: ref_voxel_grid_)
    //  {
    //    if (v > 0)
    //    {
    //      avg += v;
    //      ++count;
    //    }
    //    if (v > max)
    //      max = v;
    //  }
    //  avg /= count;
    //  LOG(INFO) << "VoxelGrid: max: " << max << ", avg: " << avg << std::endl;
}

void DepthFromDefocusNode::publishDepthmap(const cv::Mat& depth,
                                           const cv::Mat& mask) {
    static image_transport::Publisher pub =
        it_.advertise("/dvs_mapping/depthmap", 1);

    if (pub.getNumSubscribers() == 0) return;

    double min, max;
    int min_idx, max_idx;
    cv::minMaxIdx(depth, &min, &max, &min_idx, &max_idx, mask);

    cv::Mat img = 255.f * (depth - min) / (max - min);
    img.convertTo(img, CV_8U);
    cv::applyColorMap(img, img, cv::COLORMAP_JET);
    img.setTo(255, 1 - mask);
    //  cv::GaussianBlur(img, img, cv::Size(3,3), 0);

    std_msgs::Header header;
    header.stamp = event_queue_[current_event_].ts;
    sensor_msgs::ImagePtr msg =
        cv_bridge::CvImage(header, "bgr8", img).toImageMsg();
    pub.publish(msg);
}

void DepthFromDefocusNode::publishVoxelGrid(
    const evo_utils::geometry::Transformation& T_w_ref) const {
    if (pub_voxel_grid_.getNumSubscribers() > 0) {
        std_msgs::Float32MultiArray array;

        array.layout.dim.push_back(std_msgs::MultiArrayDimension());
        array.layout.dim.push_back(std_msgs::MultiArrayDimension());
        array.layout.dim.push_back(std_msgs::MultiArrayDimension());

        array.layout.data_offset = (uint32_t)0;

        array.layout.dim[0].label = "height";
        array.layout.dim[0].size = virtual_height_;
        array.layout.dim[0].stride =
            num_depth_cells_ * virtual_height_ * virtual_width_;

        array.layout.dim[1].label = "width";
        array.layout.dim[1].size = virtual_width_;
        array.layout.dim[1].stride = num_depth_cells_ * virtual_width_;

        array.layout.dim[2].label = "depth";
        array.layout.dim[2].size = num_depth_cells_;
        array.layout.dim[2].stride = num_depth_cells_;

        for (int y = 0; y < virtual_height_; ++y) {
            for (int x = 0; x < virtual_width_; ++x) {
                for (size_t k = 0; k < num_depth_cells_; ++k) {
                    array.data.push_back(voxelGridAt(ref_voxel_grid_, x, y, k));
                }
            }
        }

        geometry_msgs::Pose T_w_ref_msg;
        dvs_slam_msgs::VoxelGrid voxel_grid_msg;

        geometry_msgs::Point pos;
        geometry_msgs::Quaternion quat;
        pos.x = T_w_ref.getPosition()[0];
        pos.y = T_w_ref.getPosition()[1];
        pos.z = T_w_ref.getPosition()[2];
        quat.x = T_w_ref.getRotation().x();
        quat.y = T_w_ref.getRotation().y();
        quat.z = T_w_ref.getRotation().z();
        quat.w = T_w_ref.getRotation().w();
        T_w_ref_msg.position = pos;
        T_w_ref_msg.orientation = quat;
        voxel_grid_msg.voxel_grid = array;
        voxel_grid_msg.T_w_ref = T_w_ref_msg;
        pub_voxel_grid_.publish(voxel_grid_msg);
    }
}

void DepthFromDefocusNode::publishGlobalMap() {
    static const int accumulate_once_every =
        rpg_common_ros::param<int>(pnh_, "accumulate_local_map_once_every", 1);
    static const int skip_first =
        rpg_common_ros::param<int>(pnh_, "global_point_cloud_skip_first", 0);
    static int accumulation_cnt = 0;
    static int skipped = 0;

    if (pub_pc_global_.getNumSubscribers() > 0 && skipped >= skip_first &&
        (accumulation_cnt % accumulate_once_every == 0)) {
        *pc_global_ += *pc_;
        pc_global_->header.stamp = pc_->header.stamp;

        pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());
        pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
        pcl::toPCLPointCloud2(*pc_global_, *cloud);

        static const float voxel_filter_leaf_size =
            rpg_common_ros::param<float>(pnh_, "voxel_filter_leaf_size", .0f);

        if (voxel_filter_leaf_size > 0.0001f) {
            // apply voxel grid filter
            LOG(INFO) << "Global point cloud before filtering: "
                      << cloud->width * cloud->height << " data points ("
                      << pcl::getFieldsList(*cloud) << ").";

            // Create the filtering object
            pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
            sor.setInputCloud(cloud);

            sor.setLeafSize(voxel_filter_leaf_size, voxel_filter_leaf_size,
                            voxel_filter_leaf_size);
            sor.filter(*cloud_filtered);
            LOG(INFO) << "Global point cloud after filtering: "
                      << cloud_filtered->width * cloud_filtered->height
                      << " data points (" << pcl::getFieldsList(*cloud_filtered)
                      << ").";

            pcl::fromPCLPointCloud2(*cloud_filtered, *pc_global_);
        } else {
            pcl::fromPCLPointCloud2(*cloud, *pc_global_);
        }

        static const float radius_search =
            rpg_common_ros::param<float>(pnh_, "radius_search_global_map", .02);
        static const int min_num_neighbors =
            rpg_common_ros::param<int>(pnh_, "min_num_neighbors_global_map", 5);

        if (radius_search > 0) {
            // filter point cloud to remove outliers
            PointCloud::Ptr cloud_filtered(new PointCloud);
            pcl::RadiusOutlierRemoval<PointType> outrem;
            outrem.setInputCloud(pc_global_);
            outrem.setRadiusSearch(radius_search);
            outrem.setMinNeighborsInRadius(min_num_neighbors);
            outrem.filter(*cloud_filtered);

            pc_global_->swap(*cloud_filtered);
        }

        sensor_msgs::PointCloud2 pc_to_publish;
        pcl::toROSMsg(*pc_global_, pc_to_publish);
        pub_pc_global_.publish(pc_to_publish);
    }
    if (skipped++ >= skip_first)
        accumulation_cnt = (accumulation_cnt + 1) % accumulate_once_every;

}

void DepthFromDefocusNode::clearEventQueue() {
    static size_t event_history_size_ = 5000000;

    size_t last_event = std::min(current_event_, last_kf_update_event_);

    if (last_event > event_history_size_) {
        size_t remove_events = last_event - event_history_size_;
        event_queue_.erase(event_queue_.begin(),
                           event_queue_.begin() + remove_events);
        current_event_ -= remove_events;
        last_kf_update_event_ -= remove_events;
        newest_tracked_event_ -= remove_events;
}

bool DepthFromDefocusNode::getPoseAt(const ros::Time& t,
                                     evo_utils::geometry::Transformation& T) {

    std::string* error_msg = new std::string();

    if (!tf_->canTransform(world_frame_id_, frame_id_, t, error_msg)) {
        LOG(WARNING) << t.toNSec() << " : " << *error_msg;
        return false;
    } else {
        tf::StampedTransform tr;
        tf_->lookupTransform(world_frame_id_, frame_id_, t, tr);
        tf::transformTFToKindr(tr, &T);
        return true;
    }
}
}

}  // namespace depth_from_defocus


#pragma once
#include "lv_common.hpp"
#include "../upobj.hpp"
#include "../handle/resource.hpp"
#include "frea/src/matrix.hpp"
#include "frea/src/quaternion.hpp"
#include "frea/src/expquat.hpp"
#include "lubee/src/meta/typelist.hpp"
#include <boost/variant.hpp>

namespace rev {
	using Vec2 = frea::Vec_t<float,2,false>;
	using Vec3 = frea::Vec_t<float,3,false>;
	using Vec4 = frea::Vec_t<float,4,false>;
	struct LCVec4 : Vec4 {
		int size;

		LCVec4(const Vec2& v):
			Vec4(v.convert<4>()),
			size(2)
		{}
		LCVec4(const Vec3& v):
			Vec4(v.convert<4>()),
			size(3)
		{}
		LCVec4(const Vec4& v):
			Vec4(v),
			size(4)
		{}
	};

	using Mat2 = frea::Mat2;
	using Mat3 = frea::Mat3;
	using Mat4 = frea::Mat4;
	struct LCMat4 : UPObj<Mat4> {
		using base_t = UPObj<Mat4>;
		int	size;

		LCMat4(const Mat2& m):
			base_t(m.convert<4,4>()),
			size(2)
		{}
		LCMat4(const Mat3& m):
			base_t(m.convert<4,4>()),
			size(3)
		{}
		LCMat4(const Mat4& m):
			base_t(m),
			size(4)
		{}
	};
}
namespace std {
	template <>
	struct hash<rev::LCVec4> {
		std::size_t operator()(const rev::LCVec4& v) const noexcept {
			return std::hash<rev::Vec4>()(v);
		}
	};
	template <>
	struct hash<rev::LCMat4> {
		std::size_t operator()(const rev::LCMat4& m) const noexcept {
			return std::hash<rev::Mat4>()(*m);
		}
	};
}

namespace rev {
	#define SEQ_LCVAR \
		(LuaNil)(bool)(const char*)(lua_Number) \
		(LCVec4)(LCMat4)(frea::Quat)(frea::ExpQuat)(frea::RadF)(frea::DegF) \
		(HRes)(WRes)(Lua_SP)(LCTable_SP)(void*)(lua_CFunction)(std::string)
	using LCVar = boost::variant<BOOST_PP_SEQ_ENUM(SEQ_LCVAR)>;
	using LCVar_Types = lubee::Types<BOOST_PP_SEQ_ENUM(SEQ_LCVAR)>;
	#undef SEQ_LCVAR

	class LCValue : public LCVar {
		private:
			template <int N>
			using IConst = std::integral_constant<int, N>;
			template <class... Args, int N, ENABLE_IF((N==sizeof...(Args)))>
			static void _TupleAsTable(LCTable_SP&, const std::tuple<Args...>&, IConst<N>) {}
			template <class... Args, int N, ENABLE_IF((N != sizeof...(Args)))>
			static void _TupleAsTable(LCTable_SP& tbl, const std::tuple<Args...>& t, IConst<N>);
			template <class... Args>
			static LCTable_SP _TupleAsTable(const std::tuple<Args...>& t);
			HRes _toSPtr() const;
		public:
			struct HashVisitor : boost::static_visitor<std::size_t> {
				template <class T>
				std::size_t operator()(const T& t) const {
					return std::hash<T>()(t);
				}
			};
			LCValue();
			LCValue(const LCValue& lc);
			LCValue(LCValue&& lcv);
			LCValue(lua_OtherNumber num);
			LCValue(lua_Integer num);
			LCValue(lua_IntegerU num);
			LCValue(lua_OtherIntegerU num);
			LCValue(lua_OtherInteger num);
			LCValue(const frea::DegD& d);
			LCValue(const frea::RadD& r);
			LCValue(const Vec2& v);
			LCValue(const Vec3& v);
			LCValue(const Vec4& v);
			LCValue(const Mat2& m);
			LCValue(const Mat3& m);
			LCValue(const Mat4& m);
			template <
				class T,
				ENABLE_IF((
					LCVar_Types::Has<std::decay_t<T>>{}
				))
			>
			LCValue(T&& t): LCVar(std::forward<T>(t)) {}
			template <class T, ENABLE_IF((!LCVar_Types::Has<T*>{}))>
			LCValue(T* ptr):
				LCValue(reinterpret_cast<void*>(ptr))
			{}

			// Tupleは配列に変換
			LCValue(std::tuple<>& t);
			LCValue(std::tuple<>&& t);
			LCValue(const std::tuple<>& t);
			template <class... Args>
			LCValue(std::tuple<Args...>& t);
			template <class... Args>
			LCValue(std::tuple<Args...>&& t);
			template <class... Args>
			LCValue(const std::tuple<Args...>& t);
			template <
				class V,
				ENABLE_IF((
					!LCVar_Types::Has<std::decay_t<V>>{} &&
					frea::is_vector<V>{}
				))
			>
			LCValue(const V& v):
				LCVar(frea::Vec_t<float,V::size, false>(v))
			{}
			LCValue& operator = (const LCValue& lcv);
			LCValue& operator = (LCValue&& lcv);
			// リソースの固有ハンドルは汎用へ読み替え
			template <
				class T,
				ENABLE_IF((
					!LCVar_Types::Has<std::shared_ptr<T>>{} &&
					std::is_base_of<Resource, T>{}
				))
			>
			LCValue(const std::shared_ptr<T>& sp):
				LCVar(std::static_pointer_cast<Resource>(sp))
			{}
			//! 中身を配列とみなしてアクセス
			/*! \param[in] n 0オリジンのインデックス */
			const LCValue& operator[] (int n) const;

			//! 任意のリソースハンドルへ変換して取り出す
			template <class H>
			auto toSPtr() const {
				return std::static_pointer_cast<H>(_toSPtr());
			}
			bool preciseCompare(const LCValue& lcv) const;
			bool operator == (const LCValue& lcv) const noexcept;
			bool operator != (const LCValue& lcv) const noexcept;
			//! Nilかfalseの場合にのみfalse, それ以外はtrueを返す
			explicit operator bool () const noexcept;
			void push(lua_State* ls) const;
			const char* toCStr() const;
			std::string toString() const;
			LuaType type() const;
			template <int N>
			auto toVector() const {
				return boost::get<LCVec4>(*this).convert<N>();
			}
			template <int M, int N>
			auto toMatrix() const {
				auto& m = *boost::get<LCMat4>(*this);
				return m.convert<M,N>();
			}
			template <class A>
			A toAngle() const {
				return A(boost::get<frea::RadF>(*this));
			}
	};
	std::ostream& operator << (std::ostream& os, const LCValue& lcv);
}
namespace std {
	template <>
	struct hash<rev::LCValue> {
		std::size_t operator()(const rev::LCValue& v) const {
			return boost::apply_visitor(rev::LCValue::HashVisitor(), static_cast<const rev::LCVar&>(v));
		}
	};
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE EncodingTests

#include "popops/Encoding.hpp"
#include "poplar/IPUModel.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/TileMapping.hpp"
#include "poputil/exceptions.hpp"
#include <boost/multi_array.hpp>
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <limits>
#include <poplibs_support/TestDevice.hpp>
#include <random>
#include <type_traits>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace popops;
using namespace poplibs_support;

static inline std::vector<std::uint64_t>
getRandomIndices(std::size_t numIndices, std::size_t length,
                 bool insertIgnoreIndices) {
  std::vector<std::uint64_t> indices(numIndices);
  std::mt19937 randomEngine;
  auto maxLen = insertIgnoreIndices ? length : length - 1;
  boost::random::uniform_int_distribution<std::uint64_t> dist(0, maxLen);
  for (std::size_t i = 0; i < numIndices; i++) {
    auto index = dist(randomEngine);
    indices[i] = index == length ? MASKED_LABEL_CODE : index;
  }
  return indices;
}

static inline boost::multi_array<double, 2>
getEncodedModel(const std::vector<std::uint64_t> &indices, std::size_t length,
                double off, double on) {
  const auto numIndices = indices.size();
  boost::multi_array<double, 2> encoded(boost::extents[numIndices][length]);
  for (std::size_t i = 0; i < numIndices; ++i) {
    std::fill_n(&encoded[i][0], encoded[i].size(), off);
    if (indices[i] != MASKED_LABEL_CODE) {
      encoded[i][indices[i]] = on;
    }
  }
  return encoded;
}

template <typename IndexType>
static inline void copyIndices(const std::vector<std::uint64_t> &indices,
                               char *out) {
  auto *typed = reinterpret_cast<IndexType *>(out);
  for (std::size_t i = 0; i < indices.size(); ++i) {
    std::int64_t max =
        static_cast<std::int64_t>(std::numeric_limits<IndexType>::max());
    std::int64_t min =
        -static_cast<std::int64_t>(std::numeric_limits<IndexType>::min());
    const auto range = max + min;
    BOOST_CHECK(static_cast<int64_t>(indices[i]) <= range);
    typed[i] = static_cast<IndexType>(indices[i]);
  }
}

static inline void copyIndices(const std::vector<std::uint64_t> &indices,
                               const Type &indexType, char *mem) {
  if (indexType == UNSIGNED_INT) {
    copyIndices<unsigned>(indices, mem);
  } else if (indexType == INT) {
    copyIndices<int>(indices, mem);
  }
}

static bool encodeTest(std::size_t numIndices, std::size_t length,
                       bool insertIgnoreIndices, const Type &indicesType,
                       const Type &encodedType, bool custom) {
  auto device = createTestDevice(TEST_TARGET, 1, 4);
  const auto &target = device.getTarget();

  if (custom && insertIgnoreIndices) {
    std::cerr << "Error::Indices cannot be ignored with custom levels \n";
    return 1;
  }

  Graph graph(target);
  popops::addCodelets(graph);
  auto indices = graph.addVariable(indicesType, {numIndices}, "indices");
  poputil::mapTensorLinearly(graph, indices);

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostIndices = allocateHostMemoryForTensor(
      indices, "indices", graph, uploadProg, downloadProg, tmap);

  auto randIndices = getRandomIndices(numIndices, length, insertIgnoreIndices);
  const float off = custom ? 2 : 0;
  const float on = custom ? 4 : 1;
  auto modelEncoded = getEncodedModel(randIndices, length, off, on);
  copyIndices(randIndices, indicesType, rawHostIndices.get());

  auto prog = Sequence();
  auto encoded = graph.addVariable(encodedType, {numIndices, length},
                                   VariableMappingMethod::LINEAR, "encoded");
  if (custom) {
    auto onTensor = graph.addConstant(encodedType, {}, on);
    auto offTensor = graph.addConstant(encodedType, {}, off);
    graph.setTileMapping(onTensor, 0);
    graph.setTileMapping(offTensor, 0);
    encodeOneHot(graph, indices, encoded, prog, onTensor, offTensor,
                 "/OneHotEncodeTestCustom");
  } else {
    encodeOneHot(graph, indices, encoded, prog, "/OneHotEncodeTest");
  }
  auto rawHostEncoded = allocateHostMemoryForTensor(
      encoded, "encoded", graph, uploadProg, downloadProg, tmap);

  Engine engine(graph, Sequence(uploadProg, prog, downloadProg));
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);

    engine.run(0);
  });

  boost::multi_array<double, 2> hostEncoded(boost::extents[numIndices][length]);
  copy(target, encodedType, rawHostEncoded.get(), hostEncoded);

  // No calculation just assignment here and 0 or 1 exactly representable
  // by any type.
  constexpr double relativeTolerance = 0, absoluteTolerance = 0;
  auto matchesModel = checkIsClose("encoded", hostEncoded, modelEncoded,
                                   relativeTolerance, absoluteTolerance);
  return matchesModel;
}

template <typename T>
bool checkIota(char *out, int64_t startInteger, std::size_t length) {
  auto *output = reinterpret_cast<T *>(out);
  bool matchesModel = true;
  for (T i = 0; i != static_cast<T>(length); ++i) {
    if (startInteger + i != output[i]) {
      matchesModel = false;
    }
  }
  return matchesModel;
}

static bool iotaTest(std::int64_t startInteger, std::size_t length,
                     const Type &type) {
  auto device = createTestDevice(TEST_TARGET, 1, 4);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  Tensor iotaOut;
  if (length % 2) {
    iotaOut = graph.addVariable(type, {length}, "iotaOut");
  } else {
    iotaOut = graph.addVariable(type, {2, length / 2}, "iotaOut");
  }

  poputil::mapTensorLinearly(graph, iotaOut);

  if (length % 2 == 0) {
    iotaOut = iotaOut.transpose().flatten();
  }

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostIotaOut = allocateHostMemoryForTensor(
      iotaOut, "iotaOut", graph, uploadProg, downloadProg, tmap);

  auto prog = Sequence();
  if (type == UNSIGNED_INT) {
    unsigned start = static_cast<unsigned>(startInteger);
    iota(graph, iotaOut, start, prog, "/iotaTest");
  } else if (type == INT) {
    int start = static_cast<int>(startInteger);
    iota(graph, iotaOut, start, prog, "/iotaTest");
  }

  Engine engine(graph, Sequence(uploadProg, prog, downloadProg));
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);
    engine.run(0);
  });

  if (type == UNSIGNED_INT) {
    return checkIota<unsigned>(rawHostIotaOut.get(), startInteger, length);
  } else if (type == INT) {
    return checkIota<int>(rawHostIotaOut.get(), startInteger, length);
  } else {
    return false;
  }
}

BOOST_AUTO_TEST_CASE(UnsignedIotaTestOdd) {
  BOOST_CHECK(iotaTest(3, 101, UNSIGNED_INT));
}

BOOST_AUTO_TEST_CASE(UnsignedIotaTestEven) {
  BOOST_CHECK(iotaTest(3, 102, UNSIGNED_INT));
}

BOOST_AUTO_TEST_CASE(IntIotaTest) { BOOST_CHECK(iotaTest(-3, 121, INT)); }

#define TEST_NAME(name, n, l, ign, iType, eType, custom)                       \
  name##_##n##x##l##_##ign##_##iType##_##eType##_##custom

#define TEST_TYPE(name, n, l, ign, iType, eType, custom)                       \
  BOOST_AUTO_TEST_CASE(TEST_NAME(name, n, l, ign, iType, eType, custom)) {     \
    auto matchesModel = encodeTest(n, l, ign, iType, eType, custom);           \
    BOOST_CHECK(matchesModel);                                                 \
  }

#define ENUMERATE_VALID_TYPE_TESTS(name, n, l, ign, custom)                    \
  TEST_TYPE(name, n, l, ign, UNSIGNED_INT, FLOAT, custom)                      \
  TEST_TYPE(name, n, l, ign, UNSIGNED_INT, HALF, custom)                       \
  TEST_TYPE(name, n, l, ign, UNSIGNED_INT, UNSIGNED_INT, custom)               \
  TEST_TYPE(name, n, l, ign, UNSIGNED_INT, INT, custom)                        \
  TEST_TYPE(name, n, l, ign, INT, FLOAT, custom)                               \
  TEST_TYPE(name, n, l, ign, INT, HALF, custom)                                \
  TEST_TYPE(name, n, l, ign, INT, UNSIGNED_INT, custom)                        \
  TEST_TYPE(name, n, l, ign, INT, INT, custom)

ENUMERATE_VALID_TYPE_TESTS(EncodeOneHot, 1, 1, false, false)
ENUMERATE_VALID_TYPE_TESTS(EncodeOneHot, 20, 5, true, false)
ENUMERATE_VALID_TYPE_TESTS(EncodeOneHot, 5, 21, true, false)
ENUMERATE_VALID_TYPE_TESTS(EncodeOneHot, 5, 21, false, true)


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/common.hpp"
#include "heom/communicator.hpp"
#include "heom/distributed_command_line.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/hierarchy_partition.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_config.hpp"
#include "heom/population_dynamics_solver.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;
using num::real_format;
using heom::rank_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// init communication
	try {
		heom::communicator::init(&argc, &argv);
	} catch (...) {
		heom::handle_main_exception(std::current_exception());
	}
	const rank_t rank = heom::communicator::rank();
	const rank_t ranks = heom::communicator::ranks();

	// output compile time configuration
	if (rank == 0) {
		std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
		heom::write_compile_config(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;
	}

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::distributed_command_line command_line { argc, argv };

		std::cout << "main(..), rank " << rank << ": parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "main(..), rank " << rank << ": parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::population_dynamics_config heom_config(command_line.heom_config_filename());

		// create hierarchy graph and partitioning
		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		heom::partition_mapping partitioning;
		if (command_line.partition_mapping_filename().empty()) {
			std::cout << "main(..), rank " << rank << ": calculating partitioning..." << std::endl;
			// partitioning = create_simple_partitioning(complete_graph, rank, ranks);
			// partitioning = create_round_robin_partitioning(complete_graph, rank, ranks);
			partitioning = create_layer_partitioning(complete_graph, rank, ranks);
		} else {
			std::cout << "main(..), rank " << rank << ": creating partitioning from file '" << command_line.partition_mapping_filename() << "'..." << std::endl;
			partitioning = create_file_partitioning(complete_graph, rank, ranks, command_line.partition_mapping_filename());
		}

		const rank_t master_rank = partitioning.at(0); // NOTE: whoever owns the hierarchy top is the master rank and generates the observation

		heom::hierarchy_partition graph_partition(complete_graph, partitioning, rank, ranks);

		DEBUG_ONLY( std::cout << "main(..), rank " << rank << ": complete_graph.first_non_plus_id(), uid = " << complete_graph.first_non_plus_uid() << ", graph_partition.first_non_plus_id(), lid = " << graph_partition.first_non_plus_uid() << ", uid = " << graph_partition.lid_to_uid().at(graph_partition.first_non_plus_uid()) << std::endl; )

		heom::instance heom_instance(heom_config, heom::sites_to_states_mode_t::identity, graph_partition);

		heom::communicator comm(graph_partition, heom_instance);

		if (rank == master_rank) {
			heom_instance.set_hierarchy_top(heom_config.population_dynamics_rho_init().data());
		}

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// OpenCL range
		ocl::nd_range range {
			{}, // offset
			{1, static_cast<std::uint64_t>(graph_partition.compute_nodes())}, // static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
			{1, 1} // local size
		};

		// create solver
		solver_t solver(ocl_config, range, heom_config, heom_instance);
		if (rank == master_rank) {
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
		}

		// with create_hierarchy_mask we create mask corresponding to configured filtering strategy, then update solver
		// NOTE: .data() returns pointer to mask_t array
		heom::hierarchy_mask hierarchy_mask = heom::create_hierarchy_mask(heom_config.filtering_strategy(), complete_graph, heom_config.baths_matsubaras(), heom_config.filtering_first_layer());
		solver.update_hierarchy_mask(hierarchy_mask.data());

		std::cout << "-------------------- Hierarchy Mask Counter ----------------" << std::endl;
		heom::write_hierarchy_mask_stats(hierarchy_mask, std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// register neighbour exchange before each ODE evaluation
		using ne_action_t = heom::neighbour_exchange_action<heom::ode>;
		ne_action_t ne_action(comm, heom_instance);
		solver.ode().add_pre_evaluate_action(std::ref(ne_action));

		heom::observer_list observer_list;

		// boost::format object to re-use for output
		auto format = real_format;

		if (rank == master_rank) {
			// setup output
			observer_list = heom::make_file_observer_list(heom_config, complete_graph, solver);
			// generate first observation with initial value
			observer_list.observe(0.0);
		}

		// run the solver, print output every n steps
		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		for (int_t i = 0; i < iterations; ++i) {
			// propagate
			solver.step_forward(steps_per_iteration);

			// after propagation, we are at:
			const auto current_step = (i + 1) * steps_per_iteration;
			const real_t current_time = current_step * heom_config.solver_step_size();

			if (rank == master_rank) {
				// observe
				observer_list.observe(current_time);

				// update status
				heom::write_progress(current_step - 1, total_steps, "Calculation population dynamics: ", std::cout);
			}
		}

		// create sub-directory for profiling data inside current working directory
		boost::filesystem::path profiling_dir { "profile" };
		if (rank == master_rank) { // assuming shared filesystem
			if (!boost::filesystem::create_directory(profiling_dir)) {
				std::cout << "Warning: cannot create profiling output directory: " << profiling_dir << ", existing data might be overwritten." << std::endl;
			}
		}
		comm.barrier(); // make sure no rank is ahead of directory creation on master rank

		// globally collect profiling data for solver across all ranks on the master rank
		heom::statistics_vector solver_stats { solver.stats() }; // NOTE: implied order: step is expected come first
		std::vector<std::vector<heom::runtime_summary_record>> solver_records; // outer vector has all different statistics, inner vector all ranks
		for (const auto& current_pair : solver_stats)
			solver_records.push_back(comm.gather(master_rank, heom::runtime_summary_record(rank, current_pair.first))); // first is statistics object

		// globally collect profiling data for neighbour exchange action across all ranks on the master rank
		heom::statistics_vector ne_action_stats { ne_action.stats() };
		std::vector<std::vector<heom::runtime_summary_record>> ne_action_records; // outer vector has all different statistics, inner vector all ranks
		for (const auto& current_pair : ne_action_stats)
			ne_action_records.push_back(comm.gather(master_rank, heom::runtime_summary_record(rank, current_pair.first))); // first is statistics object

		// globally collect profiling data memory
		std::vector<std::string> memory_names { "local instance size",
		                                        "  hierarchy buffer size",
		                                        "    computed hierarchy nodes",
		                                        "    halo hierarchy nodes",
		                                        "max. heap via aligned::allocate(..)",
		                                        "max. OpenCL buffer allocations" };
		// NOTE: order must match names
		std::vector<std::vector<heom::memory_summary_record>> memory_records; // outer vector has all different statistics, inner vector all ranks
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, 1, heom_instance.allocated_byte())));
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, 1, heom_instance.size_hierarchy_byte()))); // first is statistics object
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, 1, heom_instance.size_hierarchy_top_byte() * graph_partition.compute_nodes())));
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, 1, heom_instance.size_hierarchy_top_byte() * graph_partition.halo_nodes())));
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, heom::memory::aligned::instance().allocations_max(), heom::memory::aligned::instance().allocated_byte_max())));
		memory_records.push_back(comm.gather(master_rank, heom::memory_summary_record(rank, solver.ocl_helper().allocations_max(), solver.ocl_helper().allocated_byte_max())));

		if (rank == master_rank) {
			// find rank with best and worst solver_step
			rank_t best_rank = 0;
			rank_t worst_rank = 0;
			auto& solver_step_records = solver_records[0]; // NOTE: implied order
			for (rank_t r = 0; r < ranks; ++r) {
				best_rank = solver_step_records[r].sum() < solver_records[0][best_rank].sum() ? r : best_rank; // find rank with minimum sum
				worst_rank = solver_step_records[r].sum() > solver_records[0][worst_rank].sum() ? r : worst_rank; // find rank with minimum sum
			}

			// create data structure for and write the solver summary table
			std::vector<heom::runtime_summary_table_entry> solver_table_entries;
			for (size_t i = 0; i < solver_stats.size(); ++i)
				solver_table_entries.emplace_back(solver_stats[i].second, solver_records[i][best_rank], solver_records[i][worst_rank]); // second is the name of the record
			heom::write_runtime_summary_table("Solver Runtime Summary", solver_table_entries, std::cout);

			// create data structure for and write the neighbour exchange summary table
			std::vector<heom::runtime_summary_table_entry> ne_action_table_entries;
			for (size_t i = 0; i < ne_action_stats.size(); ++i)
				ne_action_table_entries.emplace_back(ne_action_stats[i].second, ne_action_records[i][best_rank], ne_action_records[i][worst_rank]); // second is the name of the record
			heom::write_runtime_summary_table("Neighbour Exchange Runtime Summary", ne_action_table_entries, std::cout);

			// create data structure for and write the memory summary table
			std::vector<heom::memory_summary_table_entry> memory_table_entries;
			for (size_t i = 0; i < memory_names.size(); ++i)
				memory_table_entries.emplace_back(memory_names[i], memory_records[i][best_rank], memory_records[i][worst_rank]);
			heom::write_memory_summary_table("Memory Allocation Summary", memory_table_entries, std::cout);

			// write communication matrix
			heom::write_to_file((profiling_dir / "neighbour_exchange_matrix.dat").string(), std::bind(&heom::communicator::write_neighbour_exchange_table, &comm, std::placeholders::_1)); // comm.write_neighbour_exchange_table(neighbour_exchange_matrix_file)
			heom::write_to_file((profiling_dir / "neighbour_exchange_matrix_raw.dat").string(), std::bind(&heom::communicator::write_neighbour_exchange_table_raw, &comm, std::placeholders::_1)); // comm.write_neighbour_exchange_table_raw(neighbour_exchange_matrix_file)
		}

		// output detailed data for every rank separately
		const std::string runtime_stats_filename { "solver_statistics_rank_" + boost::lexical_cast<std::string>(rank) + ".dat" };
		heom::write_to_file((profiling_dir / runtime_stats_filename).string(), std::bind(&solver_t::write_runtime_stats, &solver, std::placeholders::_1)); // solver.write_runtime_stats(runtime_stats_file));

		const std::string ne_action_stats_filename { "neighbour_exchange_statistics_rank_" + boost::lexical_cast<std::string>(rank) + ".dat" };
		heom::write_to_file((profiling_dir / ne_action_stats_filename).string(), std::bind(&ne_action_t::write_runtime_stats, &ne_action, std::placeholders::_1)); // solver.write_runtime_stats(runtime_stats_file));

		// copy /proc/curproc/status file for each rank into profiling dir, if exists
		pid_t pid = getpid();
		boost::filesystem::path status_path { "/proc/" + boost::lexical_cast<std::string>(pid) + "/status" };
		boost::filesystem::path status_copy_path { profiling_dir / ("proc_status_rank_" + boost::lexical_cast<std::string>(rank)) };
		if (boost::filesystem::exists(status_path)) {
			if (boost::filesystem::exists(status_copy_path))
				boost::filesystem::remove(status_copy_path); // delete if exists, copy_file line above with overwrite_if_exists has permission issues
			//bost::filesystem::copy_file(status_path, status_copy_path, boost::filesystem::copy_option::overwrite_if_exists);
			//boost::filesystem::copy(status_path, status_copy_path);
			//boost::filesystem::permissions(status_copy_path, boost::filesystem::perms::add_perms | boost::filesystem::perms::owner_write); // make sure file has write rights, as source is read-only
			std::ifstream status_file(status_path.string());
			std::ofstream status_copy_file(status_copy_path.string());
			status_copy_file << status_file.rdbuf();
		}
	} catch (...) {
		eptr = std::current_exception();
	}

	// deinit communication
	try {
		heom::communicator::deinit();
	} catch (...) {
		heom::handle_main_exception(std::current_exception());
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	if (rank == 0) // NOTE: master_rank out of scope already
		std::cout << "main(): rank " << rank << ": application runtime: " << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


#include "hsm/hsm.h"

#include <gtest/gtest.h>

#include <boost/hana.hpp>

#include <future>
#include <memory>

using namespace ::testing;

namespace {
namespace bh = boost::hana;

class DispatchTableTests : public Test {
};

// States
struct S1 {
};
struct S2 {
};
struct S3 {
};
struct S4 {
};

// Events
struct e1 {
};

// Guards
const auto g1 = [](auto) { return true; };

// Actions
const auto a1 = [](auto event) {};

struct Defer {
    constexpr auto defer_events()
    {
        return hsm::defer(e1{});
    }
};

struct S {

    constexpr auto on_entry()
    {
        return [](auto counter) { (*counter)++; };
    }
    constexpr auto on_exit()
    {
        return [](auto counter) { (*counter)++; };
    }
};

struct SubState {
    constexpr auto make_transition_table()
    {
        // clang-format off
        return hsm::transition_table(
            // Region 0
            hsm::transition(S1 {}, hsm::event<e1> {}, g1, a1, S2 {})
        );
        // clang-format on
    }

    constexpr auto initial_state()
    {
        return hsm::initial(S1 {});
    }
};

struct MainState {
    constexpr auto make_transition_table()
    {
        // clang-format off
        return hsm::transition_table(
            // Region 0
            hsm::transition(S1 {}, hsm::event<e1> {}, g1, a1, S2 {}),
            hsm::transition(S2 {}, hsm::event<e1> {}, g1, a1, SubState {}),
            // Region 1
            hsm::transition(S3 {}, hsm::event<e1> {}, g1, a1, S4 {})
        );
        // clang-format on
    }

    constexpr auto initial_state()
    {
        return hsm::initial(S1 {}, S3 {});
    }
};

}

TEST_F(DispatchTableTests, should_count_max_regions)
{
    ASSERT_EQ(bh::size_c<2>, hsm::maxInitialStates(MainState{}));
}

TEST_F(DispatchTableTests, should_make_region_map)
{
    auto map = hsm::make_initial_state_map(MainState{});
    ASSERT_EQ(bh::size_c<2>, bh::size(bh::find(map, bh::typeid_(MainState{})).value()));
    ASSERT_EQ(bh::size_c<1>, bh::size(bh::find(map, bh::typeid_(SubState{})).value()));
}

TEST_F(DispatchTableTests, should_count_regions)
{
    std::array<std::vector<std::size_t>, hsm::maxInitialStates(MainState {})> regions;
    hsm::fill_inital_state_table(MainState{}, regions);
    ASSERT_EQ(2, regions[0].size());
    ASSERT_EQ(1, regions[1].size());

    ASSERT_EQ(0, regions[0][0]);
    ASSERT_EQ(3, regions[0][1]);

    ASSERT_EQ(0, regions[1][0]);
}

TEST_F(DispatchTableTests, should_resolve_history_state)
{
    auto historyTransition = bh::make_tuple(0, 1, 2, 3, 4, hsm::History { S {} });
    auto noHistoryTransition = bh::make_tuple(0, 1, 2, 3, 4, S {});

    ASSERT_TRUE(hsm::resolveHistory(historyTransition));
    ASSERT_FALSE(hsm::resolveHistory(noHistoryTransition));
}


#include "extractor/extraction_helper_functions.hpp"

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

BOOST_AUTO_TEST_SUITE(durations_are_valid)

using namespace osrm;
using namespace osrm::util;

BOOST_AUTO_TEST_CASE(all_necessary_test)
{
    BOOST_CHECK_EQUAL(extractor::durationIsValid("00:01"), true);
    BOOST_CHECK_EQUAL(extractor::durationIsValid("00:01:01"), true);
    BOOST_CHECK_EQUAL(extractor::durationIsValid("PT15M"), true);
}

BOOST_AUTO_TEST_CASE(common_durations_get_translated)
{
    BOOST_CHECK_EQUAL(extractor::parseDuration("00:01"), 60);
    BOOST_CHECK_EQUAL(extractor::parseDuration("00:01:01"), 61);
    BOOST_CHECK_EQUAL(extractor::parseDuration("01:01"), 3660);

    // check all combinations of iso duration tokens
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT1M1S"), 61);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT1H1S"), 3601);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT15M"), 900);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT15S"), 15);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT15H"), 54000);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT1H15M"), 4500);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT1H15M1S"), 4501);
}

BOOST_AUTO_TEST_CASE(iso_8601_durations_case_insensitive)
{
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT15m"), 900);
    BOOST_CHECK_EQUAL(extractor::parseDuration("PT1h15m"), 4500);
}

BOOST_AUTO_TEST_SUITE_END()


// #include <boost.h>
#include <bits/stdc++.h>
// #include <open3d/Open3D.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <ros/ros.h>

using namespace std;

/*
 * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is
 * time stamp). Taken from LeGO LOAM.
 */
struct PointXYZIRPYT {
  PCL_ADD_POINT4D
  PCL_ADD_INTENSITY;
  float roll;
  float pitch;
  float yaw;
  double time;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIRPYT,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
                                            intensity)(float, roll,
                                                       roll)(float, pitch,
                                                             pitch)(float, yaw,
                                                                    yaw)(double,
                                                                         time,
                                                                         time))

typedef PointXYZIRPYT PointTypePose;

class GetSurroundingCloud {
 private:
  // constants
  std::string mapDir =
      "/home/shrinivas/KGP/SLAM/catkin_ws_indy/Outputs/StereoMap/";
  float resolution = 2.0;
  float radius = 40.0;
  int num_frames=36;

  // Point Clouds
  pcl::PointCloud<pcl::PointXYZI>::Ptr globalmap, localMap, inputMap;
  pcl::PointCloud<PointTypePose>::Ptr globalPose6D;

  vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> allFrames;

  // Oct Tree
  pcl::octree::OctreePointCloudSearch<pcl::PointXYZI> octree;

  //----------------------------------------------------//
  //-------------- Utility Functions -------------------//
  // ---------------------------------------------------//
  Eigen::Matrix4f ConvertToTransformMatrix(float d[3], float q[3]) {
    Eigen::Matrix3f rotMat;

    rotMat = Eigen::AngleAxisf(q[0], Eigen::Vector3f::UnitX()) *
             Eigen::AngleAxisf(q[1], Eigen::Vector3f::UnitY()) *
             Eigen::AngleAxisf(q[2], Eigen::Vector3f::UnitZ());

    Eigen::Vector3f transMat;
    transMat << d[0], d[1], d[2];

    Eigen::Vector4f lastRow;
    lastRow << 0.0, 0.0, 0.0, 1.0;

    Eigen::Matrix4f transformMat;

    transformMat.block<3, 3>(0, 0) = rotMat;
    transformMat.block<3, 1>(0, 3) = transMat;
    transformMat.block<1, 4>(3, 0) = lastRow;

    return transformMat;
  }

  // Get Point Cloud from local filesystem and transform it
  pcl::PointCloud<pcl::PointXYZI>::Ptr getPointCloud(int index) {
    std::stringstream ss;
    ss << std::setw(6) << std::setfill('0') << index;
    std::string cloudDir = mapDir + ss.str() + "/cloud.pcd";
    pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr output(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::io::loadPCDFile<pcl::PointXYZI>(cloudDir, *tempCloud);
    float q[3], d[3];

    q[0] = globalPose6D->points[index].yaw;
    q[1] = globalPose6D->points[index].roll;
    q[2] = globalPose6D->points[index].pitch;

    d[0] = globalPose6D->points[index].z;  // z
    d[1] = globalPose6D->points[index].x;  // x
    d[2] = globalPose6D->points[index].y;  // y

    // cout<<q[0]<<" "<<q[1]<<" "<<q[2]<<"\n";
    // cout<<d[1]<<" "<<d[2]<<" "<<d[0]<<"\n";

    Eigen::Matrix4f transform = ConvertToTransformMatrix(d, q);

    pcl::transformPointCloud(*tempCloud, *output, transform);
    return output;
  }

  // Visualise a Point Cloud
  pcl::visualization::PCLVisualizer::Ptr simpleVis(
      pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud, string name) {
    pcl::visualization::PCLVisualizer::Ptr viewer(
        new pcl::visualization::PCLVisualizer(name));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZI>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    return (viewer);
  }

  // Get all surrounding transformed frames
  void getAllFrames(){
    for(int i=0;i<=num_frames;i++){
      allFrames.push_back(getPointCloud(i));
    }
  }

  //----------------------------------------------------//
  //-------------- Extractor Function ------------------//
  // ---------------------------------------------------//
  void extract_local_map(float X, float Y, float Z, float radius) {
    pcl::PointXYZI searchPoint;

    localMap.reset(new pcl::PointCloud<pcl::PointXYZI>());

    searchPoint.x = Y;
    searchPoint.y = Z;
    searchPoint.z = X;

    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch,
                            pointRadiusSquaredDistance) > 0) {
      cout << pointIdxRadiusSearch.size() << "\n";

      for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
        *localMap += *allFrames[pointIdxRadiusSearch[i]];
        // cout<<pointRadiusSquaredDistance[i]<<"\n";
      }
    }
  }

 public:
  //----------------------------------------------------//
  //-------------- Constructor -------------------------//
  // ---------------------------------------------------//
  GetSurroundingCloud() : octree(resolution) {
    globalmap.reset(new pcl::PointCloud<pcl::PointXYZI>);
    globalPose6D.reset(new pcl::PointCloud<PointTypePose>);
    localMap.reset(new pcl::PointCloud<pcl::PointXYZI>);
    inputMap.reset(new pcl::PointCloud<pcl::PointXYZI>);

    pcl::io::loadPCDFile<pcl::PointXYZI>(mapDir + "cloudKeyPoses3D.pcd",
                                        *globalmap);
    pcl::io::loadPCDFile<PointTypePose>(mapDir + "cloudKeyPoses6D.pcd",
                                        *globalPose6D);

    localMap.reset(new pcl::PointCloud<pcl::PointXYZI>);
    inputMap.reset(new pcl::PointCloud<pcl::PointXYZI>);

    octree.setInputCloud(globalmap);
    octree.addPointsFromInputCloud();

    // getAllFrames();

    // SaveFullCloud();

    cout<<"All Frames Saved. Starting Code\n";
  }

  void SaveFullCloud(){
      pcl::PointCloud<pcl::PointXYZI>::Ptr fullCloud(new pcl::PointCloud<pcl::PointXYZI>);

      for(int i=0;i<=num_frames;i++){
        *fullCloud+=*getPointCloud(i);
      }

      pcl::io::savePCDFileASCII (mapDir +"Complete_Cloud.pcd", *fullCloud);
  }

  //----------------------------------------------------//
  //-------------- Main Function -----------------------//
  // ---------------------------------------------------//
  void run() {
    // this we will get from a subscriber call back
    pcl::io::loadPCDFile<pcl::PointXYZI>(mapDir + "../Map/134.768931732.pcd",
                                        *inputMap);

    /**
     * Transform Input according to odometry input
     * Here we get odometry from Poses Vector. During real
     * run we will get it from odometry topic
     **/
    float q[3], d[3];

    q[0] = globalPose6D->points[0].yaw;
    q[1] = globalPose6D->points[0].roll;
    q[2] = globalPose6D->points[0].pitch;

    d[0] = globalPose6D->points[0].z;
    d[1] = globalPose6D->points[0].x;
    d[2] = globalPose6D->points[0].y;

    Eigen::Matrix4f transform = ConvertToTransformMatrix(d, q);

    pcl::PointCloud<pcl::PointXYZI>::Ptr inputMapTransformed(
        new pcl::PointCloud<pcl::PointXYZI>);

    pcl::transformPointCloud(*inputMap, *inputMapTransformed, transform);

    // odometry info of current point cloud
    // taken from data file
    float x = -9.0, y = 23.0, z = 0.0;

    extract_local_map(x, y, z, radius);

    pcl::visualization::PCLVisualizer::Ptr viewer, viewer1;
    viewer = simpleVis(localMap, "Local Map");
    viewer1 = simpleVis(inputMapTransformed, "Input Cloud");
    cout << localMap->width << "\n";

    while (!viewer1->wasStopped()) {
      viewer->spinOnce(100);
      viewer1->spinOnce(100);
      std::this_thread::sleep_for(100ms);
    }
  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "localise");

  GetSurroundingCloud obj;

  obj.SaveFullCloud();
}


//          Copyright Oliver Kowalke 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef CLOCK_H
#define CLOCK_H

#include <algorithm>
#include <cstddef>
#include <numeric>
#include <vector>

#include <boost/assert.hpp>
#include <boost/chrono.hpp>
#include <boost/cstdint.hpp>

typedef boost::chrono::high_resolution_clock    clock_type;
typedef clock_type::duration                    duration_type;
typedef clock_type::time_point                  time_point_type;

struct clock_overhead
{
    boost::uint64_t operator()()
    {
        time_point_type start( clock_type::now() );
        return ( clock_type::now() - start).count();
    }
};

duration_type overhead_clock()
{
    std::size_t iterations( 10);
    std::vector< boost::uint64_t >  overhead( iterations, 0);
    for ( std::size_t i = 0; i < iterations; ++i)
        std::generate(
            overhead.begin(), overhead.end(),
            clock_overhead() );
    BOOST_ASSERT( overhead.begin() != overhead.end() );
    return duration_type( std::accumulate( overhead.begin(), overhead.end(), 0) / iterations);
}

#endif // CLOCK_H


#pragma once

/*
   Quick analysis to collect the set of variables and constants that might
   define array segment boundaries.
*/

#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>

#include <crab/cfg/cfg.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>

#include <boost/unordered_map.hpp>
#include <boost/range/iterator_range.hpp>

namespace crab {

   namespace analyzer{

     template<class CFG>
     class array_segment_ops:
       public crab::iterators::killgen_operations_api
              <CFG,domains::flat_killgen_domain<typename CFG::variable_t> > {

       typedef crab::iterators::killgen_operations_api
              <CFG,domains::flat_killgen_domain<typename CFG::variable_t> >
              killgen_operations_api_t;

      public:

       typedef typename CFG::basic_block_label_t basic_block_label_t;
       typedef typename CFG::number_t N;
       typedef typename CFG::varname_t V;
       typedef typename CFG::variable_t variable_t;
       typedef typename killgen_operations_api_t::killgen_domain_t array_segment_domain_t;

      private:

       class array_segment_visitor: public cfg::statement_visitor<N,V> {
         typedef typename cfg::statement_visitor<N,V>::bin_op_t bin_op_t;
         typedef typename cfg::statement_visitor<N,V>::assign_t assign_t;
         typedef typename cfg::statement_visitor<N,V>::assume_t assume_t;
         typedef typename cfg::statement_visitor<N,V>::select_t select_t;
         typedef typename cfg::statement_visitor<N,V>::assert_t assert_t;

         typedef typename cfg::statement_visitor<N,V>::havoc_t    havoc_t;
         typedef typename cfg::statement_visitor<N,V>::unreach_t  unreach_t;
         typedef typename cfg::statement_visitor<N,V>::callsite_t callsite_t;

         typedef typename cfg::statement_visitor<N,V>::arr_assume_t arr_assume_t;
         typedef typename cfg::statement_visitor<N,V>::arr_load_t   arr_load_t;
         typedef typename cfg::statement_visitor<N,V>::arr_store_t  arr_store_t;

         // assume all statements have the same type expression_t;
         typedef typename bin_op_t::linear_expression_t linear_expression_t;
         typedef typename assume_t::linear_constraint_t linear_constraint_t;

         array_segment_domain_t get_variables (linear_expression_t e) {
           array_segment_domain_t res;
           for (auto v: e.variables()) res += v;
           return res;
         }

         array_segment_domain_t get_variables (linear_constraint_t c) {
           array_segment_domain_t res;
           for (auto v: c.variables()) res += v;
           return res;
         }

        public:

         array_segment_domain_t _indexes;

         array_segment_visitor (array_segment_domain_t inv)
             : _indexes(inv) { }

         void visit(bin_op_t &s){
           if (!(_indexes & s.lhs()).is_bottom()) {
             _indexes += get_variables(s.left());
             _indexes += get_variables(s.right());
           }
         }

         void visit(assign_t &s) {
           if (!(_indexes & s.lhs()).is_bottom()) {
             _indexes += get_variables(s.rhs());
           }
         }

         void visit(assume_t &s) {
           auto vars = get_variables(s.constraint());
           if (!(_indexes & vars).is_bottom())
             _indexes += vars;
         }

         void visit(arr_assume_t &s) {
           _indexes += get_variables(s.lb_index());
           _indexes += get_variables(s.ub_index());
         }

         void visit(arr_load_t &s) {
           _indexes += get_variables(s.index());
         }

         void visit(arr_store_t &s) {
           _indexes += get_variables(s.index());
         }

         void visit(unreach_t&) { }

         // FIXME: implement these
         void visit(select_t&) { }
         void visit(callsite_t&) { }
         void visit(havoc_t&) { }
         void visit(assert_t&) { }
       };

      public:

       array_segment_ops (CFG cfg): killgen_operations_api_t(cfg) { }

       virtual bool is_forward() override { return false;}

       virtual std::string name () override { return "array_segment";}

       virtual void init_fixpoint() override { }

       virtual array_segment_domain_t entry() override {
         return array_segment_domain_t::bottom();
       }

       virtual array_segment_domain_t merge(array_segment_domain_t d1,
                                            array_segment_domain_t d2)  override {
         return d1 | d2;
       }

       virtual array_segment_domain_t analyze (basic_block_label_t bb_id,
                                               array_segment_domain_t in) override {
         auto &bb = this->_cfg.get_node(bb_id);
         array_segment_visitor vis(in);
         for (auto &s: bb) { s.accept (&vis); }
         return vis._indexes;
       }
     };

     template<class CFG>
     class array_segmentation: public boost::noncopyable,
                               public crab::iterators::killgen_fixpoint_iterator
         <CFG, array_segment_ops<CFG> >{
      public:

       typedef array_segment_ops<CFG> array_segment_ops_t;
       typedef typename CFG::basic_block_label_t basic_block_label_t;
       typedef typename CFG::statement_t statement_t;
       typedef typename CFG::varname_t varname_t;
       typedef typename array_segment_ops_t::array_segment_domain_t array_segment_domain_t;

      private:

       typedef crab::iterators::killgen_fixpoint_iterator<CFG,array_segment_ops_t>
                killgen_fixpoint_iterator_t;
       typedef boost::unordered_map<basic_block_label_t, array_segment_domain_t> segment_map_t;

       segment_map_t _segment_map;

      public:

       array_segmentation(CFG cfg)
           : killgen_fixpoint_iterator_t(cfg) { }

       void exec() {
         this->run();

         CRAB_LOG("array-segment",
                  crab::outs() << "Array segment variables alive at the block entries\n";);

         for (auto p : boost::make_iterator_range(this->in_begin(), this->in_end())) {
           CRAB_LOG("array-segment",
                    crab::outs() << p.first << ":" << p.second << "\n";);
           _segment_map.insert(std::make_pair(p.first, p.second));
         }
         this->release_memory();
       }

       array_segment_domain_t get_variables (basic_block_label_t bb) const {
         auto it = _segment_map.find(bb);
         if (it == _segment_map.end())
           return array_segment_domain_t();
         else
           return it->second;
       }

       void write (crab_os &o) const { }
     };

     // Visitor for finding constants that might appear as array
     // segment boundaries.
     template<class CFG, class ArraySegmentDom>
     class array_constant_segment_visitor:
       public cfg::statement_visitor<typename CFG::number_t, typename CFG::varname_t> {

       typedef typename CFG::number_t N;
       typedef typename CFG::varname_t V;
       typedef typename cfg::statement_visitor<N,V>::bin_op_t   bin_op_t;
       typedef typename cfg::statement_visitor<N,V>::assign_t   assign_t;
       typedef typename cfg::statement_visitor<N,V>::assume_t   assume_t;
       typedef typename cfg::statement_visitor<N,V>::havoc_t    havoc_t;
       typedef typename cfg::statement_visitor<N,V>::unreach_t  unreach_t;
       typedef typename cfg::statement_visitor<N,V>::select_t   select_t;
       typedef typename cfg::statement_visitor<N,V>::callsite_t callsite_t;
       typedef typename cfg::statement_visitor<N,V>::assert_t   assert_t;

       typedef typename cfg::statement_visitor<N,V>::arr_assume_t arr_assume_t;
       typedef typename cfg::statement_visitor<N,V>::arr_load_t   arr_load_t;
       typedef typename cfg::statement_visitor<N,V>::arr_store_t  arr_store_t;

       typedef typename bin_op_t::linear_expression_t linear_expression_t;
       typedef typename linear_expression_t::number_t number_t;

      public:

       typedef std::vector<number_t> constant_set_t;

      private:

       ArraySegmentDom _dom;
       constant_set_t _csts;

      public:

       array_constant_segment_visitor (ArraySegmentDom dom): _dom(dom) { }

       constant_set_t get_constants () { return _csts;}

       /// XXX: we focus for now only on assignments
       void visit(assign_t &s) {
         if (!(_dom & s.lhs()).is_bottom()) {
           if (s.rhs().is_constant() && s.rhs().constant() >= 0 &&
               std::find(_csts.begin(), _csts.end(), s.rhs().constant()) == _csts.end()) {
             _csts.push_back(s.rhs().constant());
             // for decrementing loops we need to include the upper bound index i and i+1
             _csts.push_back(s.rhs().constant() + 1);
           }
         }
       }

       void visit(bin_op_t &s){}
       void visit(assume_t &s) {}
       void visit(arr_assume_t &s) {}
       void visit(arr_load_t &s) {}
       void visit(arr_store_t &s) {}
       void visit(select_t&) {}
       void visit(callsite_t&) {}
       void visit(havoc_t&) {}
       void visit(assert_t&) {}
       void visit(unreach_t&) {}
     };

  } // end namespace analyzer
} // end namespace crab


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_SYSTEM_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_SYSTEM_HPP

#include <boost/geometry/core/coordinate_system.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

namespace boost { namespace geometry {

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch {

template <typename Vector>
struct coordinate_system<vector_tag, Vector>
{
    typedef typename traits::coordinate_system<
        typename geometry::util::bare_type<Vector>::type
    >::type type;
};

//template <typename G>
//struct coordinate_system<quaternion_tag, G>
//{
//    typedef typename traits::coordinate_system<
//        typename geometry::util::bare_type<G>::type
//    >::type type;
//};
//
//template <typename G>
//struct coordinate_system<matrix_tag, G>
//{
//    typedef typename traits::coordinate_system<
//        typename geometry::util::bare_type<G>::type
//    >::type type;
//};

template <typename G>
struct coordinate_system<rotation_quaternion_tag, G>
{
    typedef typename traits::coordinate_system<
        typename geometry::util::bare_type<G>::type
    >::type type;
};

template <typename G>
struct coordinate_system<rotation_matrix_tag, G>
{
    typedef typename traits::coordinate_system<
        typename geometry::util::bare_type<G>::type
    >::type type;
};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_SYSTEM_HPP


/**----------------------------------------------------------------------------
 Copyright  2018-, UT-Battelle, LLC
 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 copyright holder 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.

 Author(s): Jay Jay Billings (billingsjj <at> ornl <dot> gov)
 -----------------------------------------------------------------------------*/
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE kelvin

#include <boost/test/included/unit_test.hpp>
#include <mfem.hpp>
#include <vector>
#include <MassMatrix.h>

using namespace std;
using namespace mfem;
using namespace Kelvin;

/**
 * This operation checks the basic functionality of the MassMatrix class.
 */
BOOST_AUTO_TEST_CASE(checkMass) {

	// Set of all nodes currently near particles
	set<int> nodeSet;
	set<int>::iterator outerIt;
	set<int>::iterator innerIt;

	// This test uses a particle mesh with four particles along the diagonal
	// at half integer steps. The full 4x4 mesh would have 16 elements and 25
	// vertices, but only the elements that have particles are created here.

	// Create some particles
	int numParticles = 4;
	vector<Kelvin::MaterialPoint> particles(numParticles);
	for (int i = 0; i < numParticles; i++) {
		particles[i].pos[0] = 0.5 + ((double) i);
		particles[i].pos[1] = 0.5 + ((double) i);
		particles[i].pos[2] = 0.5 + ((double) i);
		particles[i].mass = 1.0;
		std::cout << particles[i].pos[0] << " " << particles[i].pos[1]
				<< " " << particles[i].pos[2] << endl;
	}

	// Setup the shape matrix
	int numVerts = 25;
	SparseMatrix shapeMatrix(numParticles,numVerts);
	// Particle 1
	shapeMatrix.Add(0,0,0.25);
	nodeSet.insert(0);
	shapeMatrix.Add(0,1,0.25);
	nodeSet.insert(1);
	shapeMatrix.Add(0,5,0.25);
	nodeSet.insert(5);
	shapeMatrix.Add(0,6,0.25);
	nodeSet.insert(6);
	// Particle 2
	shapeMatrix.Add(1,6,0.25);
	shapeMatrix.Add(1,7,0.25);
	nodeSet.insert(7);
	shapeMatrix.Add(1,11,0.25);
	nodeSet.insert(11);
	shapeMatrix.Add(1,12,0.25);
	nodeSet.insert(12);
	// Particle 3
	shapeMatrix.Add(2,12,0.25);
	shapeMatrix.Add(2,13,0.25);
	nodeSet.insert(13);
	shapeMatrix.Add(2,17,0.25);
	nodeSet.insert(17);
	shapeMatrix.Add(2,18,0.25);
	nodeSet.insert(18);
	// Particle 4
	shapeMatrix.Add(3,18,0.25);
	shapeMatrix.Add(3,19,0.25);
	nodeSet.insert(19);
	shapeMatrix.Add(3,23,0.25);
	nodeSet.insert(23);
	shapeMatrix.Add(3,24,0.25);
	nodeSet.insert(24);

	// Finalize the matrix and put its columns in order from least to greatest.
	shapeMatrix.Finalize();
	shapeMatrix.SortColumnIndices();

	// Debug
	shapeMatrix.Print();

	// Create the mass matrix
	MassMatrix massMatrix(particles);
	massMatrix.assemble(shapeMatrix,nodeSet);

	// Construct the mass matrix associated with the grid nodes
	double particleMass = 1.0; // FIXME! Needs to be something real and from input.

	cout << "----- Checking all nodes" << endl;

	// Test some of the expected values of the matrix. Not worth it to test all.
	// The important thing to note here is that expectedValue1 is the expected
	// mass if one particle is near by, and expectedValued2 is is the expected
	// value if two particles are nearby.
	double expectedValue1 = 0.0625, expectedValue2 = 0.125, percentEPS = 1.0e-15;
	// 0 - One nearby particles
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(0,0),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(0,1),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(0,5),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(0,6),percentEPS);
	// 12 - Two nearby particles nearby for 12 coupled to itself, but only one
	// for the others.
	BOOST_REQUIRE_CLOSE(expectedValue2,massMatrix(12,12),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(12,13),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(12,17),percentEPS);
	BOOST_REQUIRE_CLOSE(expectedValue1,massMatrix(12,18),percentEPS);
	// 20 - No nearby particles, not in the massive node set, m_ij = 0
	BOOST_REQUIRE_CLOSE(0.0,massMatrix(20,20),percentEPS);
	BOOST_REQUIRE_CLOSE(0.0,massMatrix(20,21),percentEPS);
	BOOST_REQUIRE_CLOSE(0.0,massMatrix(20,15),percentEPS);
	BOOST_REQUIRE_CLOSE(0.0,massMatrix(20,16),percentEPS);

	// Print the values at the massive nodes for debugging.
	cout << "----- Printing massive nodes only" << endl;
	for (outerIt = nodeSet.begin(); outerIt != nodeSet.end(); outerIt++) {
		for(innerIt = nodeSet.begin(); innerIt != nodeSet.end(); innerIt++) {
			cout << "(" << *outerIt << ", " << *innerIt << ") "
					<< massMatrix(*outerIt,*innerIt) << ", ";
		}
		cout << endl;
	}

	// Check the diagonalized/lumped form
	auto lumpedMassMatrix = massMatrix.lump();
	outerIt = nodeSet.begin();
	for (int i = 0; i < lumpedMassMatrix.size(); i++) {
		double expectedValue = 0.0;
		if (*outerIt != 6 && *outerIt != 12 && *outerIt != 18) {
			expectedValue = 4.0*expectedValue1;
		} else {
			expectedValue = 4.0*expectedValue2;
		}
		cout << *outerIt << " " << lumpedMassMatrix[i] << endl;
		BOOST_REQUIRE_CLOSE(expectedValue,lumpedMassMatrix[i],percentEPS);
		outerIt++;
	}

	return;

}


#include <boost/multiprecision/number.hpp>