/* headers //{ */

// clang: MatousFormat
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include <nodelet/nodelet.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

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

#include <boost/circular_buffer.hpp>

#include <mrs_lib/param_loader.h>
#include <mrs_lib/dynamic_reconfigure_mgr.h>
#include <mrs_lib/subscribe_handler.h>
#include <mrs_lib/transformer.h>
#include <mrs_lib/geometry/cyclic.h>
/* #include <mrs_lib/vector_converter.h> */

#include <mrs_bumper/BumperConfig.h>
#include <mrs_msgs/ObstacleSectors.h>
#include <mrs_msgs/Histogram.h>

// shortcut type to the dynamic reconfigure manager template instance
typedef mrs_lib::DynamicReconfigureMgr<mrs_bumper::BumperConfig> drmgr_t;

//}

namespace mrs_bumper
{

  using ObstacleSectors = mrs_msgs::ObstacleSectors;
  using Histogram = mrs_msgs::Histogram;

  using pt_t = pcl::PointXYZ;
  using pc_t = pcl::PointCloud<pt_t>;
  using vec3_t = Eigen::Vector3f;

  using radians = mrs_lib::geometry::radians;

  class Bumper : public nodelet::Nodelet
  {
  public:
    /* onInit() method //{ */
    void onInit()
    {
      ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle();

      m_node_name = "Bumper";

      /* Load parameters from ROS //{*/
      mrs_lib::ParamLoader pl(nh, m_node_name);
      // LOAD STATIC PARAMETERS
      ROS_INFO("[Bumper]: Loading static parameters:");
      const auto uav_name = pl.loadParam2<std::string>("uav_name");
      pl.loadParam("update_rate", m_update_rate, 10.0);
      pl.loadParam("frame_id", m_frame_id);
      pl.loadParam("median_filter_size", m_median_filter_size, 1);

      pl.loadParam("depthmap/unknown_pixel_value", m_depthmap_unknown_pixel_value);
      m_depthmap_roi.x_offset = pl.loadParam2<int>("depthmap/roi/x_offset", 0);
      m_depthmap_roi.y_offset = pl.loadParam2<int>("depthmap/roi/y_offset", 0);
      m_depthmap_roi.width = pl.loadParam2<int>("depthmap/roi/width", 0);
      m_depthmap_roi.height = pl.loadParam2<int>("depthmap/roi/height", 0);
      pl.loadParam("depthmap/roi/centering", m_depthmap_roi_centering, false);
      pl.loadParam("depthmap/histogram_n_bins", m_depthmap_hist_n_bins, 1000);
      pl.loadParam("depthmap/histogram_quantile_area", m_depthmap_hist_quantile_area, 200);
      pl.loadParam("depthmap/max_depth", m_depthmap_max_depth);
      pl.loadParam("depthmap/camera_offset", m_depthmap_camera_offset);

      pl.loadParam("lidar3d/voxel_size", m_lidar3d_voxel_size);
      pl.loadParam("lidar3d/voxel_minpoints", m_lidar3d_voxel_minpoints);
      pl.loadParam("lidar3d/exclude_box/use", m_lidar3d_exclude_box_use);
      pl.loadMatrixStatic("lidar3d/exclude_box/offset", m_lidar3d_exclude_box_offset);
      pl.loadMatrixStatic("lidar3d/exclude_box/size", m_lidar3d_exclude_box_size);
      pl.loadParam("lidar3d/include_box/use", m_lidar3d_include_box_use);
      pl.loadMatrixStatic("lidar3d/include_box/offset", m_lidar3d_include_box_offset);
      pl.loadMatrixStatic("lidar3d/include_box/size", m_lidar3d_include_box_size);

      pl.loadParam("lidar2d/filter_size", m_lidar2d_filter_size);

      const auto fallback_timeout = pl.loadParam2<double>("fallback/timeout", 0.0);
      pl.loadParam("fallback/n_horizontal_sectors", m_fallback_n_horizontal_sectors, 0);
      pl.loadParam("fallback/vertical_fov", m_fallback_vertical_fov, 0.0);
      const auto path_to_mask = pl.loadParam2<std::string>("path_to_mask", std::string());

      // LOAD DYNAMIC PARAMETERS
      // CHECK LOADING STATUS
      if (!pl.loadedSuccessfully())
      {
        ROS_ERROR("Some compulsory parameters were not loaded successfully, ending the node");
        ros::shutdown();
      }

      m_drmgr_ptr = std::make_unique<drmgr_t>(nh, m_node_name);
      //}

      /* Create publishers and subscribers //{ */
      // Initialize subscribers
      mrs_lib::SubscribeHandlerOptions shopts;
      shopts.nh = nh;
      shopts.node_name = "Bumper";

      mrs_lib::construct_object(m_depthmap_sh, shopts, "depthmap_in");
      mrs_lib::construct_object(m_depth_cinfo_sh, shopts, "depth_cinfo_in");
      mrs_lib::construct_object(m_lidar3d_sh, shopts, "lidar3d_in");
      mrs_lib::construct_object(m_lidar2d_sh, shopts, "lidar2d_in");
      mrs_lib::construct_object(m_lidar1d_down_sh, shopts, "lidar1d_down_in");
      mrs_lib::construct_object(m_lidar1d_up_sh, shopts, "lidar1d_up_in");

      // Initialize publishers
      m_obstacles_pub = nh.advertise<ObstacleSectors>("obstacle_sectors", 1);
      m_processed_depthmap_pub = nh.advertise<sensor_msgs::Image>("processed_depthmap", 1);
      m_depthmap_hist_pub = nh.advertise<Histogram>("depthmap_histogram", 1);
      m_lidar3d_processed = nh.advertise<sensor_msgs::PointCloud2>("lidar3d_processed", 1);

      m_tf_listener_ptr = std::make_unique<tf2_ros::TransformListener>(m_tf_buffer);
      //}

      /* Initialize other varibles //{ */
      if (path_to_mask.empty())
      {
        ROS_INFO("[%s]: Not using image mask", ros::this_node::getName().c_str());
      } else
      {
        m_depthmap_mask_im = cv::imread(path_to_mask, cv::IMREAD_GRAYSCALE);
        if (m_depthmap_mask_im.empty())
        {
          ROS_ERROR("[%s]: Error loading image mask from file '%s'! Ending node.", ros::this_node::getName().c_str(), path_to_mask.c_str());
          ros::shutdown();
        } else if (m_depthmap_mask_im.type() != CV_8UC1)
        {
          ROS_ERROR("[%s]: Loaded image mask has unexpected type: '%u' (expected %u)! Ending node.", ros::this_node::getName().c_str(), m_depthmap_mask_im.type(),
                    CV_8UC1);
          ros::shutdown();
        }
      }

      if (fallback_timeout != 0.0)
      {
        if (m_fallback_n_horizontal_sectors == 0 || m_fallback_vertical_fov == 0.0)
        {
          ROS_ERROR(
              "[%s]: Fallback timeout was specified, but fallback number of horizontal sectors (%d) or fallback vertical field of view (%.2f) is invalid (both "
              "have to be > 0). Ending node.",
              m_node_name.c_str(), m_fallback_n_horizontal_sectors, m_fallback_vertical_fov);
          ros::shutdown();
        }
      }
      m_fallback_timeout = ros::Duration(fallback_timeout);

      m_depthmap_roi_initialized = false;
      m_first_message_received = false;
      m_sectors_initialized = false;
      m_lidar2d_offset_initialized = false;

      m_tfm = mrs_lib::Transformer(m_node_name, uav_name);
      //}

      m_main_loop_timer = nh.createTimer(ros::Rate(m_update_rate), &Bumper::main_loop, this);

      std::cout << "----------------------------------------------------------" << std::endl;
    }
    //}

    /* main_loop() method //{ */
    void main_loop([[maybe_unused]] const ros::TimerEvent& evt)
    {
      /* initialize some stuff, if possible //{ */

      /* Initialize number of horizontal sectors etc from camera info message //{ */
      if (!m_sectors_initialized && m_depth_cinfo_sh.hasMsg())
      {
        ROS_INFO("[Bumper]: Processing camera info message to initialize sectors");

        const auto cinfo = m_depth_cinfo_sh.getMsg();
        initialize_roi(cinfo->width, cinfo->height);

        const double w = m_depthmap_roi.width;
        const double h = m_depthmap_roi.height;
        const double fx = cinfo->K[0];
        const double fy = cinfo->K[4];
        const double horizontal_fov = std::atan2(w / 2.0, fx) * 2.0;
        const double vertical_fov = std::atan2(h / 2.0, fy) * 2.0;
        const int n_horizontal_sectors = std::ceil(2.0 * M_PI / horizontal_fov);
        initialize_sectors(n_horizontal_sectors, vertical_fov);

        ROS_INFO("[Bumper]: Depth camera horizontal FOV: %.1fdeg", horizontal_fov / M_PI * 180.0);
        ROS_INFO("[Bumper]: Depth camera vertical FOV: %.1fdeg", vertical_fov / M_PI * 180.0);
        ROS_INFO("[Bumper]: Number of horizontal sectors: %d", m_n_horizontal_sectors);
      }
      //}

      /* Initialize horizontal angle offset of 2D lidar from a new message //{ */
      if (!m_lidar2d_offset_initialized && m_lidar2d_sh.hasMsg())
      {
        ROS_INFO_THROTTLE(1.0, "[Bumper]: Initializing 2D lidar horizontal angle offset");

        const auto lidar2d_msg = m_lidar2d_sh.getMsg();
        initialize_lidar2d_offset(lidar2d_msg);

        if (m_lidar2d_offset_initialized)
          ROS_INFO("[Bumper]: 2D lidar horizontal angle offset: %.2f", m_lidar2d_offset);
        else
          ROS_WARN_THROTTLE(1.0, "[Bumper]: 2D lidar horizontal angle offset initialization failed, will retry.");
      }
      //}

      //}

      /* apply changes from dynamic reconfigure //{ */
      if (m_median_filter_size != m_drmgr_ptr->config.median_filter_size)
      {
        if (m_drmgr_ptr->config.median_filter_size > 0)
        {
          m_median_filter_size = m_drmgr_ptr->config.median_filter_size;
          update_filter_sizes();
        } else
        {
          ROS_ERROR("[Bumper]: Size of median filter cannot be <= 0: %d! Ignoring new value.", m_drmgr_ptr->config.median_filter_size);
        }
      }

      if (m_update_rate != m_drmgr_ptr->config.update_rate)
      {
        if (m_drmgr_ptr->config.update_rate > 0.0)
        {
          m_update_rate = m_drmgr_ptr->config.update_rate;
          m_main_loop_timer.setPeriod(ros::Duration(1.0 / m_update_rate));
        } else
        {
          ROS_ERROR("[Bumper]: Update rate cannot be <= 0: %lf! Ignoring new value.", m_drmgr_ptr->config.update_rate);
        }
      }
      //}

      if (m_sectors_initialized)
      {
        std::vector<std::tuple<int, std::vector<double>, ros::Time>> sensors_sectors;
        std::vector<std::string> sensors_topics;
        /* process any new messages from sensors //{ */

        // Check data from the depthmap image
        if (m_depthmap_sh.newMsg() && m_depth_cinfo_sh.hasMsg())
        {
          cv_bridge::CvImagePtr source_msg = cv_bridge::toCvCopy(m_depthmap_sh.getMsg(), std::string("16UC1"));
          if (!m_depthmap_roi_initialized)
            initialize_roi(source_msg->image.cols, source_msg->image.rows);
          const auto obstacle_sectors = find_obstacles_depthmap(source_msg);
          sensors_sectors.push_back({ObstacleSectors::SENSOR_DEPTH, obstacle_sectors, source_msg->header.stamp});
          sensors_topics.push_back(m_depthmap_sh.topicName());
        }

        // Check data from the 3D lidar
        if (m_lidar3d_sh.newMsg())
        {
          const auto cloud = m_lidar3d_sh.getMsg();
          std::vector<double> obstacle_sectors = find_obstacles_pointcloud(cloud);
          ros::Time msg_stamp;
          pcl_conversions::fromPCL(cloud->header.stamp, msg_stamp);
          sensors_sectors.push_back({ObstacleSectors::SENSOR_LIDAR3D, obstacle_sectors, msg_stamp});
          sensors_topics.push_back(m_lidar3d_sh.topicName());
        }

        // Check data from the horizontal 2D lidar
        if (m_lidar2d_offset_initialized && m_lidar2d_sh.newMsg())
        {
          const auto msg = m_lidar2d_sh.getMsg();
          std::vector<double> obstacle_sectors = find_obstacles_lidar2d(msg);
          sensors_sectors.push_back({ObstacleSectors::SENSOR_LIDAR2D, obstacle_sectors, msg->header.stamp});
          sensors_topics.push_back(m_lidar2d_sh.topicName());
        }

        // Check data from the down-facing lidar
        if (m_lidar1d_down_sh.newMsg())
        {
          const auto msg = m_lidar1d_down_sh.getMsg();
          std::vector<double> obstacle_sectors = find_obstacles_lidar1d(msg, m_bottom_sector_idx);
          sensors_sectors.push_back({ObstacleSectors::SENSOR_LIDAR1D, obstacle_sectors, msg->header.stamp});
          sensors_topics.push_back(m_lidar1d_down_sh.topicName());
        }

        // Check data from the up-facing lidar
        if (m_lidar1d_up_sh.newMsg())
        {
          const auto msg = m_lidar1d_up_sh.getMsg();
          std::vector<double> obstacle_sectors = find_obstacles_lidar1d(msg, m_top_sector_idx);
          sensors_sectors.push_back({ObstacleSectors::SENSOR_LIDAR1D, obstacle_sectors, msg->header.stamp});
          sensors_topics.push_back(m_lidar1d_up_sh.topicName());
        }

        //}

        std::vector<double> res_obstacles(m_n_total_sectors, ObstacleSectors::OBSTACLE_NO_DATA);
        std::vector<int8_t> res_sensors(m_n_total_sectors, ObstacleSectors::SENSOR_NONE);
        ros::Time res_stamp = ros::Time::now();
        /* put the resuls from different sensors together //{ */

        for (const auto& sensor_sectors : sensors_sectors)
        {
          const auto [sensor, sectors, stamp] = sensor_sectors;
          for (size_t sect_it = 0; sect_it < m_n_total_sectors; sect_it++)
          {
            const auto obstacle_dist = sectors.at(sect_it);
            // check if an obstacle was detected (*obstacle_sure*)
            const auto obstacle_unknown = obstacle_dist == ObstacleSectors::OBSTACLE_NO_DATA;
            auto& cur_value = res_obstacles.at(sect_it);
            auto  cur_unknown = cur_value == ObstacleSectors::OBSTACLE_NO_DATA;
            auto& cur_sensor = res_sensors.at(sect_it);
            // If the previous obstacle information in this sector is unknown or a closer
            // obstacle was detected by this sensor, update the information.
            if (!obstacle_unknown && (cur_value > obstacle_dist || cur_unknown))
            {
              cur_value = obstacle_dist;
              cur_sensor = sensor;
              if (res_stamp > stamp)
                res_stamp = stamp;
            }
          }
        }

        //}

        // filter the obstacles using a median filter
        res_obstacles = filter_sectors(res_obstacles);

        /* Prepare and publish the ObstacleSectors message to be published //{ */
        {
          ObstacleSectors obst_msg;
          obst_msg.header.frame_id = m_frame_id;
          obst_msg.header.stamp = res_stamp;
          obst_msg.n_horizontal_sectors = m_n_horizontal_sectors;
          obst_msg.sectors_vertical_fov = m_vertical_fov;
          obst_msg.sectors = res_obstacles;
          obst_msg.sector_sensors = res_sensors;
          m_obstacles_pub.publish(obst_msg);
        }
        //}

        /* print out some info to the console //{ */

        {
          std::vector<std::string> used_sensors;
          for (const auto& el : sensors_sectors)
          {
            const auto sensor = std::get<0>(el);
            switch (sensor)
            {
              case ObstacleSectors::SENSOR_NONE:
                used_sensors.push_back("\033[1;31minvalid \033[0m"); break;
              case ObstacleSectors::SENSOR_DEPTH:
                used_sensors.push_back("depthmap"); break;
              case ObstacleSectors::SENSOR_LIDAR1D:
                used_sensors.push_back("LiDAR 1D"); break;
              case ObstacleSectors::SENSOR_LIDAR2D:
                used_sensors.push_back("LiDAR 2D"); break;
              case ObstacleSectors::SENSOR_LIDAR3D:
                used_sensors.push_back("LiDAR 3D"); break;
            }
          }
          std::stringstream ss;
          for (size_t it = 0; it < used_sensors.size(); it++)
            ss << std::endl << "\t[" << used_sensors.at(it) << "] at topic \"" << sensors_topics.at(it) << "\"";
          ROS_INFO_STREAM_THROTTLE(2.0, "[Bumper]: Updating bumper using sensors:" << ss.str());
        }

        //}
      } else // if the number of sectors and vertical FOV were not initialized yet, check for fallback timeout
      {
        /* check for fallback timeout, apply if neccessary //{ */
        /* if we got a first sensor message, but still no cinfo, remember the stamp (for fallback timeout) //{ */

        if (!m_first_message_received && m_depthmap_sh.hasMsg())
        {
          m_first_message_stamp = m_depthmap_sh.getMsg()->header.stamp;
          m_first_message_received = true;
        }
        if (!m_first_message_received && m_lidar3d_sh.hasMsg())
        {
          pcl_conversions::fromPCL(m_lidar3d_sh.getMsg()->header.stamp, m_first_message_stamp);
          m_first_message_received = true;
        }
        if (!m_first_message_received && m_lidar2d_sh.hasMsg())
        {
          m_first_message_stamp = m_lidar2d_sh.getMsg()->header.stamp;
          m_first_message_received = true;
        }
        if (!m_first_message_received && m_lidar1d_down_sh.hasMsg())
        {
          m_first_message_stamp = m_lidar1d_down_sh.getMsg()->header.stamp;
          m_first_message_received = true;
        }
        if (!m_first_message_received && m_lidar1d_up_sh.hasMsg())
        {
          m_first_message_stamp = m_lidar1d_up_sh.getMsg()->header.stamp;
          m_first_message_received = true;
        }

        //}

        /* if the realsense timeout has run out, apply fallback values //{ */
        const ros::Duration cinfo_delay = ros::Time::now() - m_first_message_stamp;
        if (m_first_message_received && cinfo_delay >= m_fallback_timeout)
        {
          ROS_WARN("[%s]: No camera info message received after %.2f seconds, using fallback number of horizontal sectors: %d!", m_node_name.c_str(),
                   cinfo_delay.toSec(), m_fallback_n_horizontal_sectors);
          initialize_sectors(m_fallback_n_horizontal_sectors, m_fallback_vertical_fov);
          m_sectors_initialized = true;
        }
        //}
        //}
      }
    }
    //}

  private:
    // --------------------------------------------------------------
    // |                ROS-related member variables                |
    // --------------------------------------------------------------

    /* Parameters, loaded from ROS //{ */
    double m_update_rate;
    std::string m_frame_id;
    int m_median_filter_size;

    int m_depthmap_unknown_pixel_value;
    sensor_msgs::RegionOfInterest m_depthmap_roi;
    bool m_depthmap_roi_centering;
    int m_depthmap_hist_n_bins;
    int m_depthmap_hist_quantile_area;
    double m_depthmap_max_depth;
    double m_depthmap_camera_offset;

    double m_lidar2d_filter_size;

    int m_lidar3d_voxel_minpoints;
    double m_lidar3d_voxel_size;
    bool m_lidar3d_exclude_box_use;
    Eigen::Vector3d m_lidar3d_exclude_box_offset;
    Eigen::Vector3d m_lidar3d_exclude_box_size;
    bool m_lidar3d_include_box_use;
    Eigen::Vector3d m_lidar3d_include_box_offset;
    Eigen::Vector3d m_lidar3d_include_box_size;

    ros::Duration m_fallback_timeout;
    int m_fallback_n_horizontal_sectors;
    double m_fallback_vertical_fov;
    //}

    /* ROS related variables (subscribers, timers etc.) //{ */
    std::unique_ptr<drmgr_t> m_drmgr_ptr;

    mrs_lib::SubscribeHandler<sensor_msgs::Image> m_depthmap_sh;
    mrs_lib::SubscribeHandler<sensor_msgs::CameraInfo> m_depth_cinfo_sh;
    mrs_lib::SubscribeHandler<pc_t> m_lidar3d_sh;
    mrs_lib::SubscribeHandler<sensor_msgs::LaserScan> m_lidar2d_sh;
    mrs_lib::SubscribeHandler<sensor_msgs::Range> m_lidar1d_down_sh;
    mrs_lib::SubscribeHandler<sensor_msgs::Range> m_lidar1d_up_sh;

    tf2_ros::Buffer m_tf_buffer;
    std::unique_ptr<tf2_ros::TransformListener> m_tf_listener_ptr;

    ros::Publisher m_obstacles_pub;
    ros::Publisher m_processed_depthmap_pub;
    ros::Publisher m_depthmap_hist_pub;
    ros::Publisher m_lidar3d_processed;

    ros::Timer m_main_loop_timer;

    std::string m_node_name;
    //}

  private:
    // --------------------------------------------------------------
    // |                   Other member variables                   |
    // --------------------------------------------------------------

    /* Misc. member variables //{ */
    uint32_t m_n_horizontal_sectors;
    uint32_t m_bottom_sector_idx;
    uint32_t m_top_sector_idx;
    using angle_range_t = std::pair<double, double>;
    std::vector<angle_range_t> m_horizontal_sector_ranges;
    std::vector<boost::circular_buffer<double>> m_sector_filters;
    uint32_t m_n_total_sectors;
    double m_vertical_fov;
    float m_vertical_halffov_sin;

    cv::Mat m_depthmap_mask_im;
    bool m_depthmap_roi_initialized;
    ros::Time m_first_message_stamp;
    bool m_first_message_received;
    bool m_sectors_initialized;

    bool m_lidar2d_offset_initialized;
    double m_lidar2d_offset;
    //}

    mrs_lib::Transformer m_tfm;

  private:
    // --------------------------------------------------------------
    // |                 Obstacle detection methods                 |
    // --------------------------------------------------------------

    /* find_obstacles_depthmap() method //{ */
    std::vector<double> find_obstacles_depthmap(cv_bridge::CvImagePtr source_msg)
    {
      std::vector<double> ret(m_n_total_sectors, ObstacleSectors::OBSTACLE_NO_DATA);

      /* Apply ROI //{ */
      cv::Rect roi(m_depthmap_roi.x_offset, m_depthmap_roi.y_offset, m_depthmap_roi.width, m_depthmap_roi.height);
      source_msg->image = source_msg->image(roi);
      //}

      /* Prepare the image for obstacle detection //{ */
      // create the detection image
      cv::Mat detect_im = source_msg->image.clone();
      cv::Mat raw_im = source_msg->image;
      cv::Mat known_pixels;
      if (m_depthmap_unknown_pixel_value != std::numeric_limits<uint16_t>::max())
      {
        cv::compare(detect_im, m_depthmap_unknown_pixel_value, known_pixels, cv::CMP_NE);
      }

      // dilate and erode the image if requested
      {
        const int elem_a = m_drmgr_ptr->config.structuring_element_a;
        const int elem_b = m_drmgr_ptr->config.structuring_element_b;
        cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(elem_a, elem_b), cv::Point(-1, -1));
        cv::dilate(detect_im, detect_im, element, cv::Point(-1, -1), m_drmgr_ptr->config.dilate_iterations);
        cv::erode(detect_im, detect_im, element, cv::Point(-1, -1), m_drmgr_ptr->config.erode_iterations);

        // erode without using zero (unknown) pixels
        if (m_drmgr_ptr->config.erode_ignore_empty_iterations > 0)
        {
          cv::Mat unknown_as_max = detect_im;
          if (m_depthmap_unknown_pixel_value != std::numeric_limits<uint16_t>::max())
          {
            unknown_as_max = cv::Mat(raw_im.size(), CV_16UC1, std::numeric_limits<uint16_t>::max());
            detect_im.copyTo(unknown_as_max, known_pixels);
          }
          cv::erode(unknown_as_max, detect_im, element, cv::Point(-1, -1), m_drmgr_ptr->config.erode_ignore_empty_iterations);
        }
      }
      //}

      // TODO: filter out ground?

      m_depthmap_hist_n_bins = m_drmgr_ptr->config.histogram_n_bins;
      m_depthmap_hist_quantile_area = m_drmgr_ptr->config.histogram_quantile_area;
      m_depthmap_max_depth = m_drmgr_ptr->config.max_depth;

      cv::Mat usable_pixels;
      if (m_depthmap_mask_im.empty())
        usable_pixels = known_pixels;
      else
        cv::bitwise_and(known_pixels, m_depthmap_mask_im, usable_pixels);

      const double depthmap_to_meters = 1.0 / 1000.0;
      const double bin_max = m_depthmap_max_depth / depthmap_to_meters;
      const double bin_size = bin_max / m_depthmap_hist_n_bins;
      const auto hist = calculate_histogram(detect_im, m_depthmap_hist_n_bins, bin_max, usable_pixels);
      const int quantile = find_histogram_quantile(hist, m_depthmap_hist_quantile_area);
      double obstacle_dist = quantile * bin_size * depthmap_to_meters;
      if (quantile == m_depthmap_hist_n_bins)
        obstacle_dist = ObstacleSectors::OBSTACLE_NOT_DETECTED;

      if (m_processed_depthmap_pub.getNumSubscribers() > 0)
      {
        /* Create and publish the debug image //{ */
        cv_bridge::CvImagePtr processed_depthmap_cvb = source_msg;
        processed_depthmap_cvb->image = detect_im;
        sensor_msgs::ImageConstPtr out_msg = processed_depthmap_cvb->toImageMsg();
        m_processed_depthmap_pub.publish(out_msg);
        //}
      }

      if (m_depthmap_hist_pub.getNumSubscribers() > 0)
      {
        /* Create and publish the debug image //{ */
        Histogram out_msg;
        out_msg.unit = "mm";
        out_msg.bin_size = bin_size;
        out_msg.bin_max = bin_max;
        out_msg.bin_min = 0;
        out_msg.bin_mark = quantile;
        out_msg.bins = hist;
        m_depthmap_hist_pub.publish(out_msg);
        //}
      }

      ret.at(0) = obstacle_dist;
      return ret;
    }
    //}

    /* find_obstacles_lidar1d() method //{ */
    std::vector<double> find_obstacles_lidar1d(const sensor_msgs::Range::ConstPtr& msg, const int sector)
    {
      std::vector<double> ret(m_n_total_sectors, ObstacleSectors::OBSTACLE_NO_DATA);

      double obstacle_dist = msg->range;
      if (obstacle_dist <= msg->min_range || obstacle_dist >= msg->max_range)
        obstacle_dist = ObstacleSectors::OBSTACLE_NOT_DETECTED;
      ret.at(sector) = obstacle_dist;

      return ret;
    }
    //}

    /* find_obstacles_lidar2d() method //{ */
    /* double buffer_max(const boost::circular_buffer<double>& buffer) */
    /* { */
    /*   double max = std::numeric_limits<double>::lowest(); */
    /*   for (const auto& val : buffer) */
    /*   { */
    /*     if (val > max) */
    /*       max = val; */
    /*   } */
    /*   return max; */
    /* } */

    std::vector<double> find_obstacles_lidar2d(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
    {
      std::vector<double> ret(m_n_total_sectors, ObstacleSectors::OBSTACLE_NO_DATA);
      const auto buffer_length = m_lidar2d_filter_size;
      // check minimal obstacle distance for each horizontal sector
      for (size_t it = 0; it < m_n_horizontal_sectors; it++)
      {
        const auto& cur_angle_range = m_horizontal_sector_ranges.at(it);
        double min_range = std::numeric_limits<double>::max();
        // buffer of the last *buffer_length* measurements
        boost::circular_buffer<double> buffer(buffer_length);
        for (unsigned ray_it = 0; ray_it < scan_msg->ranges.size(); ray_it++)
        {
          const double ray_range = scan_msg->ranges.at(ray_it) + buffer_length / 2 * scan_msg->angle_increment;
          const double ray_angle = scan_msg->angle_min + ray_it * scan_msg->angle_increment + m_lidar2d_offset - buffer_length / 2 * scan_msg->angle_increment;
          // check if the ray is in the current horizontal sector
          if (angle_in_range(ray_angle, cur_angle_range))
          {
            buffer.push_back(ray_range);
            /* const double cur_max_range = buffer_max(buffer); */
            const double cur_max_range = *std::max_element(std::begin(buffer), std::end(buffer));
            // If the buffer has *buffer_length* measurements and maximal distance
            // in the buffer is lower than *min_range*, update *min_range*.
            // This should filter out solitary false detections of the laser rangefinder,
            // which would otherwise trigger the repulsion failsafe mechanism.
            if (buffer.full() && cur_max_range < min_range)
              min_range = cur_max_range;
          }
        }
        if (min_range < scan_msg->range_min || min_range > scan_msg->range_max)
          min_range = ObstacleSectors::OBSTACLE_NOT_DETECTED;
        ret.at(it) = min_range;
      }
      return ret;
    }
    //}

    /* find_obstacles_pointcloud() method //{ */
    std::vector<double> find_obstacles_pointcloud(const pc_t::ConstPtr& cloud_orig)
    {
      std::vector<double> ret(m_n_total_sectors, ObstacleSectors::OBSTACLE_NO_DATA);

      auto cloud = boost::make_shared<pc_t>();

      /* reduce the number of points using VoxelGrid (output to cloud) //{ */

      {
        pcl::VoxelGrid<pt_t> vg;
        vg.setInputCloud(cloud_orig);
        vg.setMinimumPointsNumberPerVoxel(m_lidar3d_voxel_minpoints);
        vg.setLeafSize(m_lidar3d_voxel_size, m_lidar3d_voxel_size, m_lidar3d_voxel_size);
        vg.filter(*cloud);
      }

      //}

      /* crop out points belonging to the UAV and too far points (include box and exclude box) //{ */

      {
        pcl::CropBox<pt_t> cb;

        if (m_lidar3d_exclude_box_use)
        {
          const Eigen::Vector3d box_point1 = m_lidar3d_exclude_box_offset + m_lidar3d_exclude_box_size/2.0;
          const Eigen::Vector3d box_point2 = m_lidar3d_exclude_box_offset - m_lidar3d_exclude_box_size/2.0;
          const Eigen::Vector4f bpt1(box_point1.x(), box_point1.y(), box_point1.z(), 1.0f);
          const Eigen::Vector4f bpt2(box_point2.x(), box_point2.y(), box_point2.z(), 1.0f);
          cb.setMax(bpt1);
          cb.setMin(bpt2);
          cb.setInputCloud(cloud);
          cb.setNegative(true);
          cb.filter(*cloud);
        }

        if (m_lidar3d_include_box_use)
        {
          const Eigen::Vector3d box_point1 = m_lidar3d_include_box_offset + m_lidar3d_include_box_size/2.0;
          const Eigen::Vector3d box_point2 = m_lidar3d_include_box_offset - m_lidar3d_include_box_size/2.0;
          const Eigen::Vector4f bpt1(box_point1.x(), box_point1.y(), box_point1.z(), 1.0f);
          const Eigen::Vector4f bpt2(box_point2.x(), box_point2.y(), box_point2.z(), 1.0f);
          cb.setMax(bpt1);
          cb.setMin(bpt2);
          cb.setInputCloud(cloud);
          cb.setNegative(false);
          cb.filter(*cloud);
        }
      }

      //}

      m_lidar3d_processed.publish(cloud);

      // transform the pointcloud to the untilted frame
      auto cloud_tfd_opt = m_tfm.transformSingle(m_frame_id, cloud);
      if (cloud_tfd_opt == std::nullopt)
        return ret;
      cloud = cloud_tfd_opt.value();

      for (const auto& el : *cloud)
      {
        const vec3_t ray(el.x, el.y, el.z);
        const auto [sector, dist] = sector_obstacle(ray);
        // if the obstacle isn't in any sector, skip the point
        if (sector < 0)
          continue;
        // otherwise, replace the minimum in the result list, if applicable
        auto& cur = ret.at(sector);
        if (cur == ObstacleSectors::OBSTACLE_NO_DATA || cur > dist)
          cur = dist;
      }

      return ret;
    }
    //}

  private:
    // --------------------------------------------------------------
    // |                       Helper methods                       |
    // --------------------------------------------------------------

    /* initialize_sectors() method //{ */
    // initializes some helper variables related to the sectors
    void initialize_sectors(int n_horizontal_sectors, double vfov)
    {
      m_n_horizontal_sectors = n_horizontal_sectors;
      m_bottom_sector_idx = n_horizontal_sectors;
      m_top_sector_idx = n_horizontal_sectors + 1;
      m_horizontal_sector_ranges = initialize_ranges(n_horizontal_sectors);
      m_n_total_sectors = n_horizontal_sectors + 2;
      m_vertical_fov = vfov;
      m_vertical_halffov_sin = std::sin(m_vertical_fov/2.0);
      update_filter_sizes();
      m_sectors_initialized = true;
    }
    //}

    /* initialize_ranges() method //{ */
    // initializes angle ranges of horizontal sectors
    std::vector<angle_range_t> initialize_ranges(uint32_t m_n_horizontal_sectors)
    {
      std::vector<angle_range_t> ret;
      ret.reserve(m_n_horizontal_sectors);
      for (unsigned sector_it = 0; sector_it < m_n_horizontal_sectors; sector_it++)
        ret.push_back(get_horizontal_sector_angle_interval(sector_it));
      return ret;
    }
    //}

    /* get_horizontal_sector_angle_interval() method //{ */
    // helper method for initialization of angle ranges of horizontal sectors
    angle_range_t get_horizontal_sector_angle_interval(unsigned sector_it)
    {
      assert(sector_it < m_n_horizontal_sectors);
      const double angle_step = 2.0 * M_PI / m_n_horizontal_sectors;
      const double angle_start = radians::wrap(sector_it * angle_step - angle_step / 2.0);
      const double angle_end = radians::wrap(angle_start + angle_step);
      return {angle_start, angle_end};
    }
    //}

    /* angle_in_range() method //{ */
    bool angle_in_range(double angle, const angle_range_t& angle_range)
    {
      angle = radians::wrap(angle);
      bool in_range = angle > angle_range.first && angle < angle_range.second;
      // corner-case for the first sector (which would have angle_range.first < 0.0, but it is normalized as angle_range.first + 2.0*M_PI)
      if (angle_range.first > angle_range.second)
        in_range = angle < angle_range.second || angle > angle_range.first;
      return in_range;
    }
    //}

    /* update_filter_sizes() method //{ */
    void update_filter_sizes()
    {
      const boost::circular_buffer<double> init_bfr(m_median_filter_size, ObstacleSectors::OBSTACLE_NO_DATA);
      m_sector_filters.resize(m_n_total_sectors, init_bfr);
      for (auto& fil : m_sector_filters)
        fil.rset_capacity(m_median_filter_size);
    }
    //}

    // | ------ helper methods for dephmap obstacle detection ----- |
    /* initialize_roi() method //{ */
    // initializes the image region of interest using parameters, loaded from ROS, and given dimensions of a received image
    void initialize_roi(unsigned img_width, unsigned img_height)
    {
      if (m_depthmap_roi_centering)
      {
        if (m_depthmap_roi.height == 0)
          m_depthmap_roi.height = img_height;
        if (m_depthmap_roi.height > img_height)
        {
          ROS_ERROR("[%s]: Desired ROI height (%d) is larger than image height (%d) - clamping!", m_node_name.c_str(), m_depthmap_roi.height, img_height);
          m_depthmap_roi.height = img_height;
        }

        if (m_depthmap_roi.width == 0)
          m_depthmap_roi.width = img_width;
        if (m_depthmap_roi.width > img_width)
        {
          ROS_ERROR("[%s]: Desired ROI width (%d) is larger than image width (%d) - clamping!", m_node_name.c_str(), m_depthmap_roi.width, img_width);
          m_depthmap_roi.width = img_width;
        }

        m_depthmap_roi.y_offset = (img_height - m_depthmap_roi.height) / 2;
        m_depthmap_roi.x_offset = (img_width - m_depthmap_roi.width) / 2;
      }
      if (m_depthmap_roi.y_offset + m_depthmap_roi.height > unsigned(img_height) || m_depthmap_roi.height == 0)
        m_depthmap_roi.height = std::clamp(int(img_height - m_depthmap_roi.y_offset), 0, int(img_height));
      if (m_depthmap_roi.x_offset + m_depthmap_roi.width > unsigned(img_width) || m_depthmap_roi.width == 0)
        m_depthmap_roi.width = std::clamp(int(img_width), 0, int(img_width));
      m_depthmap_roi_initialized = true;
    }
    //}

    /* calculate_histogram() method //{ */
    using hist_t = std::vector<float>;
    hist_t calculate_histogram(const cv::Mat& img, const int n_bins, const double bin_max, const cv::Mat& mask)
    {
      assert(((img.type() & CV_MAT_DEPTH_MASK) == CV_16U) || ((img.type() & CV_MAT_DEPTH_MASK) == CV_16S));
      int histSize[] = {n_bins};
      float range[] = {0, (float)bin_max};
      const float* ranges[] = {range};
      int channels[] = {0};
      cv::MatND hist;
      calcHist(&img, 1, channels, mask, hist, 1, histSize, ranges);
      return hist;
    }
    //}

    /* find_histogram_quantile() method //{ */
    int find_histogram_quantile(const hist_t& hist, const double quantile_area)
    {
      double cur_area = 0;
      for (unsigned it = 0; it < hist.size(); it++)
      {
        cur_area += hist[it];
        if (cur_area > quantile_area)
          return it;
      }
      return hist.size();
    }
    //}

    // | ----- helper methods for 2D lidar obstacle detection ----- |
    /* initialize_lidar2d_offset() method //{ */
    // initializes an angle offset of a 2D LiDAR using transforms
    void initialize_lidar2d_offset(sensor_msgs::LaserScan::ConstPtr lidar2d_msg)
    {
      geometry_msgs::Vector3Stamped x_lidar;
      x_lidar.header = lidar2d_msg->header;
      x_lidar.vector.x = 1.0;
      x_lidar.vector.y = x_lidar.vector.z = 0.0;
      geometry_msgs::Vector3 x_fcu;
      const auto tfd_opt = m_tfm.transformSingle(m_frame_id, x_lidar);
      if (!tfd_opt.has_value())
        return;
      m_lidar2d_offset = std::atan2(tfd_opt->vector.y, tfd_opt->vector.x);
      m_lidar2d_offset_initialized = true;
    }
    //}

    // | ----- helper methods for 3D lidar obstacle detection ----- |
    /* sector_obstacle() method //{ */
    // rel_point: point, relative to the UAV (in the untilted coordinate frame)
    // returns: index of the sector in which the point lies and its distance
    std::pair<int, float> sector_obstacle(const vec3_t& rel_point)
    {
      const auto dist = rel_point.norm();
      const auto vert_sin = rel_point.z()/dist;
      if (vert_sin > m_vertical_halffov_sin)
      {
        return {m_top_sector_idx, dist};
      }
      else if (vert_sin < -m_vertical_halffov_sin)
      {
        return {m_bottom_sector_idx, dist};
      }
      else
      {
        const auto hori_angle = std::atan2(rel_point.y(), rel_point.x());

        for (size_t it = 0; it < m_n_horizontal_sectors; it++)
        {
          if (angle_in_range(hori_angle, m_horizontal_sector_ranges.at(it)))
            return {it, dist};
        }
      }
      return {ObstacleSectors::OBSTACLE_NO_DATA, ObstacleSectors::OBSTACLE_NO_DATA};
    }
    //}

    // | ----------- helper methods for median filtering ---------- |
    /* get_median() method //{ */
    template <typename T>
    static T get_median(const boost::circular_buffer<T>& buffer)
    {
      // copy the buffer to a helper vector
      std::vector<T> data;
      data.reserve(buffer.size());
      for (const auto& el : buffer)
        data.push_back(el);
      // sort the vector up to the nth element
      std::nth_element(std::begin(data), std::begin(data)+data.size()/2, std::end(data));
      // get the nth element (that's the median)
      const T median = *(std::begin(data)+data.size()/2);
      if (std::isinf(median))
        ROS_WARN("[Bumper]: median is inf...");
      return median;
    }
    //}

    /* print_median_buffer() method //{ */
    template <typename T>
    void print_median_buffer(boost::circular_buffer<T> fil, T median, unsigned n)
    {
      std::cout << "Buffer #" << n << ": " << median << std::endl;
      for (unsigned it = 0; it < fil.size(); it++)
        std::cout << it << "\t";
      std::cout << std::endl;
      for (const auto& v : fil)
        std::cout << std::fixed << std::setprecision(3) << v << "\t";
      std::cout << std::endl;
    }
    //}

    /* filter_sectors() method //{ */
    // applies one step of a median filter on each sector and returns the result
    template <typename T>
    std::vector<T> filter_sectors(const std::vector<T>& sectors)
    {
      assert(sectors.size() == m_sector_filters.size());
      std::vector<T> ret;
      ret.reserve(sectors.size());
      for (unsigned it = 0; it < sectors.size(); it++)
      {
        const T sec = sectors.at(it);
        auto& fil = m_sector_filters.at(it);
        fil.push_back(sec);
        const auto median = get_median(fil);
        ret.push_back(median);
#ifdef DEBUG_MEDIAN_FILTER
        print_median_buffer(fil, median, it);
#endif
      }
      return ret;
    }
    //}

  };  // class Bumper
};    // namespace mrs_bumper

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mrs_bumper::Bumper, nodelet::Nodelet)


//          Copyright Rein Halbersma 2010-2020.
// 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)

#include <core/board/group.hpp>                 // action::is_realized, make_group
#include <core/board/sequence.hpp>              // nano, micro, checkers, Roman, spantsiretti, international, frisian, ktar<10, 11>,
                                                // ktar<10, 12>, Compact_10_12, Compact_12_10, basic_board<12, 10>, canadian, srilankan, dumm
#include <dctl/core/board/angle.hpp>            // _deg, inverse, rotate
#include <dctl/core/board/coordinates.hpp>      // operator==, rotate, to_sco
#include <boost/range/irange.hpp>               // irange
#include <boost/test/unit_test.hpp>             // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE, BOOST_CHECK
#include <algorithm>                            // all_of
#include <type_traits>                          // common_type
#include <vector>                               // vector

using namespace dctl::core;
using namespace literals;

BOOST_AUTO_TEST_SUITE(GridCoordinates)

BOOST_AUTO_TEST_CASE_TEMPLATE(GroupActionIsRealizedForAllCyclicGroupsOnAllSquareCoordinates, T, BoardSequence)
{
        constexpr auto op = [](auto i, auto j) { return rotate(i, j); };
        constexpr auto inv = [](auto i) { return inverse(i); };

        auto const C1 = make_group(
                { 0_deg },
                op, inv
        );

        auto const C2 = make_group(
                { 0_deg, 180_deg },
                op, inv
        );

        auto const C4 = make_group(
                { 0_deg,  90_deg, 180_deg, 270_deg },
                op, inv
        );

        using CyclicGroup = std::common_type_t<decltype(C1), decltype(C2), decltype(C4)>;

        auto const C_N = std::vector<CyclicGroup>
        {
                C1, C2, C4
        };

        auto const squares = boost::irange(0, T::size());
        BOOST_CHECK(
                std::all_of(C_N.begin(), C_N.end(), [=](auto const& g) {
                        return std::all_of(squares.begin(), squares.end(), [&](auto i) {
                                auto const coord = to_sco(i, T::inner_grid);
                                return group::action::is_realized(coord, g);
                        });
                })
        );
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/tokenizer.hpp>
#include <iostream>
#include "urlearning/score_cache/score_cache.h"
#include "urlearning/score_cache/best_score_calculator.h"
#include "urlearning/score_cache/best_score_creator.h"
#include "urlearning/base/typedefs.h"

using namespace std;

std::pair<float,float>
get_dag_score( const char* model_filename
                   , std::vector<bestscorecalculators::BestScoreCalculator*> & spgs
		   , bool verbose
		   , int & num_edges
		   , int & num_edges_to_remove
		   , int & num_edges_to_remove_alt
                   )
{
    typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
    boost::char_separator<char> sep(", \n\r");
    std::ifstream input_file(model_filename, std::ios_base::in);
    if(not input_file.good())
    {
        cerr << "Invalid model file " << model_filename << endl;
        return std::pair<float,float>(0.0,0.0);
    }
    std::string line;
    int varIdx = 0;
    std::getline(input_file, line);
    tokenizer toks(line,sep);
    int variableCount = 0;
    for(auto iter=toks.begin(); iter != toks.end(); iter++)
        variableCount++;
    int edges[64][64];
    for(int i=0; i<variableCount; i++)
        for(int j=0; j<variableCount; j++)
	    edges[i][j] = 0;
    float total_score = 0.0;
    num_edges = 0;
    num_edges_to_remove = 0;
    num_edges_to_remove_alt = 0;
    std::vector<uint64_t> alt_parents(variableCount, 0);
    char msg[1024] = {0};
    char msg_alt[1024] = {0};
    int N = 0;
    int N_alt = 0;
    do{
        uint64_t parents = 0;
        tokenizer tok(line,sep);
        int num=0;
        for(auto iter=tok.begin(); iter != tok.end(); iter++)
        {
                float value = atof(iter->c_str());
                if(std::abs(value) > 1e-5)
		{
                  parents |=  uint64_t(1) << num  ; // variable #num is a parent of variable #varIdx
		  num_edges++;
		  alt_parents[num] |= uint64_t(1) << varIdx;
		  edges[varIdx][num] = 1;
		  edges[num][varIdx] = 1;
		}
                num++;
        }
	// if score available
	if(spgs[varIdx])
	{
	  total_score += spgs[varIdx]->getScore(parents) ;
	  uint64_t optimal_parents = spgs[varIdx]->getParents();
	  uint64_t bit_diff = parents ^ optimal_parents;
	  int diff_bits = cardinality(bit_diff);
	  num_edges_to_remove +=  diff_bits;
	  if(diff_bits)
	  {
	    for(int j=0; j<variableCount;j++)
	    {
		uint64_t mask = uint64_t(1) << j;
		if(bit_diff & mask)
		  N += snprintf(msg+N, 24, "(%d -> %d),", j, varIdx);
	    }
	  }
	}
        varIdx++;
    }while(std::getline(input_file, line) and varIdx < variableCount );
    input_file.close();

    float alt_score = 0.0;
    for(int i=0; i<variableCount; i++)
    {
	if(spgs[i])
	{
	  alt_score += spgs[i]->getScore(alt_parents[i]);
	  uint64_t optimal_parents = spgs[i]->getParents();
	  uint64_t bit_diff = alt_parents[i] ^ optimal_parents;
	  int diff_bits = cardinality(bit_diff);
	  num_edges_to_remove_alt += diff_bits;
          if(diff_bits)
          {
            for(int j=0; j<variableCount;j++)
            {
                uint64_t mask = uint64_t(1) << j;
                if(bit_diff & mask)
                  N_alt += snprintf(msg_alt+N_alt, 24, "(%d -> %d),", j, i);
            }
          }
	}
    }
    num_edges = 0;
    for(int i=0; i<variableCount; i++)
	for(int j=i+1; j<variableCount; j++)
	    num_edges += edges[i][j];

    if(verbose and total_score < alt_score and msg[0])
      printf("\nSuggest_reg to remove edges %d, scores reg %f alt %f, %s in %s\n", num_edges_to_remove, total_score, alt_score, msg, model_filename);
    else if(verbose and alt_score < total_score and msg_alt[0])
      printf("\nSuggest_alt to remove edges %d, scores reg %f alt %f, %s in %s\n", num_edges_to_remove_alt,total_score, alt_score,  msg_alt, model_filename);

    return std::pair<float,float>(total_score, alt_score);
}

// read score file .pss, and multiple DAGs, print score for each DAG
//
int main(int argc, char** argv)
{
  const std::string score_file_name ( argv[1] );

  char** dag_files = argv + 2;
  const int num_models = argc - 2;

  std::ifstream sf(score_file_name);
  const bool score_file_good = sf.good();
  if(score_file_good)
    sf.close();
  scoring::ScoreCache cache;
  if(score_file_good)
    cache.read(score_file_name);

  std::string bestScoreCalculator = "list";
  std::vector<bestscorecalculators::BestScoreCalculator*> spgs(60,NULL);
  if(score_file_good)
    spgs = bestscorecalculators::create(bestScoreCalculator, cache);

  std::vector<std::string> models( dag_files, dag_files + num_models );
  for(int i=0; i< num_models; i++)
  {
        if(models[i].rfind('/') != std::string::npos)
          models[i] = models[i].substr(models[i].rfind('/')+1);
	if(models[i].find(".csv") != std::string::npos)
	  models[i] = models[i].substr(0,models[i].find(".csv"));

        transform(models[i].begin(), models[i].end(), models[i].begin(), ::toupper);
  }

  bool verbose = false;
  float scores[num_models] = {0.0};
  for(int i=0; i<num_models; i++)
  {
    int num_edges = 0;
    int num_edges_to_remove = 0;
    int num_edges_to_remove_alt = 0;
    std::pair<float,float> dag_scores = get_dag_score(dag_files[i], spgs, verbose, num_edges, num_edges_to_remove, num_edges_to_remove_alt);

    scores[i] = dag_scores.first > dag_scores.second ? dag_scores.second : dag_scores.first ;
    int to_remove = dag_scores.first > dag_scores.second ? num_edges_to_remove : num_edges_to_remove_alt ;

    //float diff = scores[i] - scores[0];
    //float rel_diff = scores[0] ? diff/abs(scores[0]) : 0.0;
    if( 0 == i )
      printf("%s  %f  edges %d  ", models[i].c_str(), scores[i], num_edges);
    else
      printf("%s  %f  edges %d remove %d ", models[i].c_str(), scores[i], num_edges, to_remove);
  }
  printf("\n");
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Molecule.hpp"

#include <algorithm>
#include <iostream>
#include <string>
#include <vector>
#include <cmath>
#include <iomanip>

#include "Config.hpp"

#include <boost/format.hpp>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "Atom.hpp"
#include "cavity/Element.hpp"
#include "MathUtils.hpp"
#include "Symmetry.hpp"

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph)
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph)
{
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(0, 0, 0, 0);
}

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph,
                   int nr_gen, int gen[3])
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph)
{
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(nr_gen, gen[0], gen[1], gen[2]);
}

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph,
                   const Symmetry & pg)
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph),
      pointGroup_(pg)
{
    rotor_ = findRotorType();
}

Molecule::Molecule(const std::vector<Sphere> & sph)
    : nAtoms_(sph.size()), spheres_(sph)
{
    charges_ = Eigen::VectorXd::Ones(nAtoms_);
    masses_.resize(nAtoms_);
    geometry_.resize(Eigen::NoChange, nAtoms_);
    for (size_t i = 0; i < nAtoms_; ++i) {
        masses_(i) = spheres_[i].radius;
        geometry_.col(i) = spheres_[i].center;
        double charge = charges_(i);
        double mass = masses_(i);
        atoms_.push_back( Atom("Dummy", "Du", charge, mass, mass, geometry_.col(i)) );
    }
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(0, 0, 0, 0);
}

Molecule::Molecule(const Molecule &other)
{
    *this = other;
}

Eigen::Vector3d Molecule::centerOfMass()
{
    Eigen::Vector3d com;
    com << 0.0, 0.0, 0.0;
    for (size_t i = 0; i < nAtoms_; ++i) {
        com += masses_(i) * atoms_[i].position;
    }
    com *= 1.0/masses_.sum();
    return com;
}

Eigen::Matrix3d Molecule::inertiaTensor()
{
    Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero();

    for (size_t i = 0; i < nAtoms_; ++i) {
        // Diagonal
        inertia(0,0) += masses_(i) * (geometry_(1,i) * geometry_(1,i) + geometry_(2,
                                      i) * geometry_(2,i));
        inertia(1,1) += masses_(i) * (geometry_(0,i) * geometry_(0,i) + geometry_(2,
                                      i) * geometry_(2,i));
        inertia(2,2) += masses_(i) * (geometry_(0,i) * geometry_(0,i) + geometry_(1,
                                      i) * geometry_(1,i));

        // Off-diagonal
        inertia(0,1) -= masses_(i) * (geometry_(0,i) * geometry_(1,i));
        inertia(0,2) -= masses_(i) * (geometry_(0,i) * geometry_(2,i));
        inertia(1,2) -= masses_(i) * (geometry_(1,i) * geometry_(2,i));
    }
    // Now symmetrize
    hermitivitize(inertia);

    // Check elements for a numerical zero and make it a hard zero
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            if (fabs(inertia(i,j)) < 1.0e-14) {
                inertia(i,j) = 0.0;
            }
        }
    }

    return inertia;
}

rotorType Molecule::findRotorType()
{
    rotorType type;
    if (nAtoms_ == 1) {
        type = rtAtom;
    } else {
        // Get inertia tensor
        Eigen::Matrix3d inertia = inertiaTensor();
        // Diagonalize inertia tensor V^t * I * V
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
        if (eigenSolver.info() != Eigen::Success) abort();
        // Determine the degeneracy of the eigenvalues.
        int deg = 0;
        double tmp, abs, rel;
        for (int i = 0; i < 2; ++i) {
            for (int j = i + 1; j < 3 && deg < 2; ++j) { // Check i and j != i
                abs = fabs(eigenSolver.eigenvalues()[i] - eigenSolver.eigenvalues()[j]);
                tmp = eigenSolver.eigenvalues()[j]; // Because the eigenvalues are already in ascending order.
                if (abs > 1.0e-14) {
                    rel = abs/tmp;
                } else {
                    rel = 0.0;
                }
                if (rel < 1.0e-8) {
                    ++deg;
                }
            }
        }
        // Get the rotor type based on the degeneracy.
        if (eigenSolver.eigenvalues()[0] == 0.0) {
            type = rtLinear;
        } else if (deg == 2) {
            type = rtSpherical;
        } else if (deg == 1) { // We do not distinguish between prolate and oblate.
            type = rtSymmetric;
        } else {
            type = rtAsymmetric;
        }
    }

    return type;
}

void Molecule::translate(const Eigen::Vector3d &translationVector)
{
    // Translate the geometry_ matrix and update the geometric data in atoms_.
    for (size_t i = 0; i < nAtoms_; ++i) {
        geometry_.col(i) -= translationVector;
        Eigen::Vector3d tmp = geometry_.col(i);
        atoms_[i].position = tmp;
    }
}

void Molecule::moveToCOM()
{
    Eigen::Vector3d com = centerOfMass();
    this->translate(com);
}

void Molecule::rotate(const Eigen::Matrix3d &rotationMatrix)
{
    // Rotate the geometry_ matrix and update the geometric data in atoms_.
    geometry_ *=
        rotationMatrix; // The power of Eigen: geometry_ = geometry_ * rotationMatrix;
    for (size_t i = 0; i < nAtoms_; ++i) {
        Eigen::Vector3d tmp = geometry_.col(i);
        atoms_[i].position = tmp;
    }
}

void Molecule::moveToPAF()
{
    Eigen::Matrix3d inertia = inertiaTensor();
    // Diagonalize inertia tensor V^t * I * V
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
    if (eigenSolver.info() != Eigen::Success) abort();
    // Rotate to Principal Axes Frame
    this->rotate(eigenSolver.eigenvectors());
    std::cout << eigenSolver.eigenvalues() << std::endl;
}

Molecule& Molecule::operator=(const Molecule& other)
{
    // Self assignment is bad
    if (this == &other)
        return *this;

    nAtoms_ = other.nAtoms_;
    charges_ = other.charges_;
    masses_ = other.masses_;
    geometry_ = other.geometry_;
    atoms_ = other.atoms_;
    spheres_ = other.spheres_;
    rotor_ = other.rotor_;
    pointGroup_ = other.pointGroup_;

    return *this;
}

std::ostream & operator<<(std::ostream &os, const Molecule &m)
{
    if (m.nAtoms_ != 0) {
      os << "                 Geometry (in Angstrom)" << std::endl;
      os << "   Center            X             Y             Z     \n";
      os << "------------   ------------  ------------  ------------\n";
      for (size_t i = 0; i < m.nAtoms_; ++i) {
        os << boost::format("%|=12s|") % m.atoms_[i].symbol;
        os << boost::format("   %10.6f  ") % (m.geometry_(0, i) * bohrToAngstrom());
        os << boost::format("  %10.6f  ")  % (m.geometry_(1, i) * bohrToAngstrom());
        os << boost::format("  %10.6f  ")  % (m.geometry_(2, i) * bohrToAngstrom());
        os << std::endl;
      }
      os << "Rotor type: " << rotorTypeList[m.rotor_];
    } else {
      os << "  No atoms in this molecule!" << std::endl;
    }

    return os;

}

Eigen::VectorXd computeMEP(const Molecule & mol, const std::vector<Element> & el)
{
    Eigen::VectorXd mep = Eigen::VectorXd::Zero(el.size());
    for (size_t i = 0; i < mol.nAtoms(); ++i) {
        for (size_t j = 0; j < el.size(); ++j) {
            double dist = (mol.geometry().col(i) - el[j].center()).norm();
            mep(j) += mol.charges(i) / dist;
        }
    }
    return mep;
}

Eigen::VectorXd computeMEP(const std::vector<Element> & el, double charge, const Eigen::Vector3d & origin)
{
    Eigen::VectorXd mep = Eigen::VectorXd::Zero(el.size());
    for (size_t i = 0; i < el.size(); ++i) {
        double dist = (origin - el[i].center()).norm();
        mep(i) += charge / dist;
    }
    return mep;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The electron analog elastic scattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace MonteCarlo{

//! The scattering distribution base class
class AnalogElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                    public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the  elastic distribution
  typedef Teuchos::Array<Utility::Pair< double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElasticDistribution;

  //! Constructor
  AnalogElasticElectronScatteringDistribution(
        const ElasticDistribution& elastic_scattering_distribution,
        const double lower_cutoff_angle = 1.0e-6,
        const bool angle_is_used_as_independent_variable = true );

  //! Destructor
  virtual ~AnalogElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  double evaluatePDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the distribution
  double evaluate( const unsigned incoming_energy_bin,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluate( const double incoming_energy,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluatePDF( const unsigned incoming_energy_bin,
                      const double scattering_angle ) const;

  //! Evaluate the CDF
  double evaluateCDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the cross section ratio for the cutoff angle
  double evaluateCutoffCrossSectionRatio( const double incoming_energy ) const;

  //! Return the energy at a given energy bin
  double getEnergy( const unsigned energy_bin ) const;

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle_cosine ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle_cosine,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

//protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

private:

  // The scattering angle above which the analog distribution is used
  double d_lower_cutoff_angle;

  // Independent parameter flag: false = angle cosine, true = angle (in units of pi)
  bool d_angle_is_used_as_independent_variable;

  // elastic scattering distribution without forward screening data
  ElasticDistribution d_elastic_scattering_distribution;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   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.
 */

#ifndef DART_GUI_TRACKBALL_HPP_
#define DART_GUI_TRACKBALL_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace dart {
namespace gui {

/// \brief
class Trackball
{
public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// \brief Default constructor
  Trackball();

  /// \brief Constructor
  Trackball(const Eigen::Vector2d& _center, double _radius);

  /// \brief Set the starting position to the project of (x,y) on the trackball
  void startBall(double _x, double _y);

  /// \brief Update the current rotation to rotate from mStartPos to the
  ///        projection of (x,y) on trackball, then update mStartPos
  void updateBall(double _x, double _y);

  /// \brief Apply the current rotation to openGL env
  void applyGLRotation();

  /// \brief Draw the trackball on screen
  void draw(int _winWidth, int _winHeight);

  /// \brief
  void setTrackball(const Eigen::Vector2d& _center, const double _radius);

  /// \brief
  void setCenter(const Eigen::Vector2d& _center);

  /// \brief
  void setRadius(const double _radius);

  /// \brief
  void setQuaternion(const Eigen::Quaterniond& _q);

  /// \brief
  Eigen::Quaterniond getCurrQuat() const;

  /// \brief
  Eigen::Matrix3d getRotationMatrix() const;

  /// \brief
  Eigen::Vector2d getCenter() const;

  /// \brief
  double getRadius() const;

private:
  /// \brief Project screen coordinate (x,y) to the trackball
  Eigen::Vector3d mouseOnSphere(double _mouseX, double _mouseY) const;

  /// \brief Compute the quaternion that rotates from vector "from" to vector
  ///        "to"
  Eigen::Quaterniond quatFromVectors(
      const Eigen::Vector3d& _from, const Eigen::Vector3d& _to) const;

  /// \brief
  Eigen::Vector2d mCenter;

  /// \brief
  double mRadius;

  /// \brief
  Eigen::Vector3d mStartPos;

  /// \brief
  Eigen::Quaterniond mCurrQuat;
};

} // namespace gui
} // namespace dart

#endif // DART_GUI_TRACKBALL_HPP_


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/power.hpp>
#include <boost/units/systems/si/solid_angle.hpp>
#include <boost/units/systems/si/temperature.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

/// \file
/// CODATA recommended values of fundamental physico-chemical constants
/// CODATA 2006 values as of 2007/03/30

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

// PHYSICO-CHEMICAL
/// Avogadro constant
BOOST_UNITS_PHYSICAL_CONSTANT(N_A,quantity<inverse_amount>,6.02214179e23/mole,3.0e16/mole);
/// atomic mass constant
BOOST_UNITS_PHYSICAL_CONSTANT(m_u,quantity<mass>,1.660538782e-27*kilograms,8.3e-35*kilograms);
/// Faraday constant
BOOST_UNITS_PHYSICAL_CONSTANT(F,quantity<electric_charge_over_amount>,96485.3399*coulombs/mole,2.4e-3*coulombs/mole);
/// molar gas constant
BOOST_UNITS_PHYSICAL_CONSTANT(R,quantity<energy_over_temperature_amount>,8.314472*joules/kelvin/mole,1.5e-5*joules/kelvin/mole);
/// Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(k_B,quantity<energy_over_temperature>,1.3806504e-23*joules/kelvin,2.4e-29*joules/kelvin);
/// Stefan-Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(sigma_SB,quantity<power_over_area_temperature_4>,5.670400e-8*watts/square_meter/pow<4>(kelvin),4.0e-13*watts/square_meter/pow<4>(kelvin));
/// first radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_1,quantity<power_area>,3.74177118e-16*watt*square_meters,1.9e-23*watt*square_meters);
/// first radiation constant for spectral radiance
BOOST_UNITS_PHYSICAL_CONSTANT(c_1L,quantity<power_area_over_solid_angle>,1.191042759e-16*watt*square_meters/steradian,5.9e-24*watt*square_meters/steradian);
/// second radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_2,quantity<length_temperature>,1.4387752e-2*meter*kelvin,2.5e-8*meter*kelvin);
/// Wien displacement law constant : lambda_max T
BOOST_UNITS_PHYSICAL_CONSTANT(b,quantity<length_temperature>,2.8977685e-3*meter*kelvin,5.1e-9*meter*kelvin);
/// Wien displacement law constant : nu_max/T
BOOST_UNITS_PHYSICAL_CONSTANT(b_prime,quantity<frequency_over_temperature>,5.878933e10*hertz/kelvin,1.0e15*hertz/kelvin);

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP


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

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_successor successor
 *
 * \par Description
 * With one parameter it is equivalent to \c next
 * It is in the type \c A0, the least  \c A0 elementwise strictly greater than  \c a0.
 * \par
 * With two parameters, the second is an integer value  \c n
 * and the result is equivalent to applying \c next \c abs(n) times to  \c a0.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/successor.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template<class A0> inline
 *   A0 predecessor(A0 const& a0);
 *
 *   template<class A0,class A1> inline
 *   A0 predecessor(A0 const& a0,const A1& n);
 * }
 * \endcode
 *
 * \param a0 the first parameter of successor
 * \param a1 the second parameter of successor
 *
 * \return a value of type A0
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag successor_ of functor successor
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct successor_ : ext::elementwise_<successor_> { typedef ext::elementwise_<successor_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::successor_, successor, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::successor_, successor, 2)
} }

#endif

// modified by jt the 25/12/2010


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================

#include <boost/config.hpp>
#include <boost/concept_archetype.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/graph_archetypes.hpp>

typedef boost::default_constructible_archetype<
  boost::sgi_assignable_archetype<> > dist_value;

// So generate_infinity works...
namespace std {
  template <>
  class numeric_limits<dist_value> {
  public:
    static dist_value max BOOST_PREVENT_MACRO_SUBSTITUTION () {
      return boost::static_object<dist_value>::get();
    }
  };
}

dist_value abs(const dist_value& x) { return x; }
std::size_t abs(std::size_t x) { return x; }

int main()
{
  using namespace boost;
  typedef default_constructible_archetype<
    sgi_assignable_archetype<
    equality_comparable_archetype<> > > vertex_t;
  {
    typedef incidence_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag> IncidenceGraph;
    typedef vertex_list_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag, IncidenceGraph> graph_t;
    graph_t& g = static_object<graph_t>::get();
    vertex_t s;
    typedef graph_traits<graph_t>::edge_descriptor edge_t;
    readable_property_map_archetype<edge_t, std::size_t> weight;
    readable_property_map_archetype<vertex_t, int> index;
    read_write_property_map_archetype<vertex_t, std::size_t> distance;
    dijkstra_shortest_paths(g, s,
                            vertex_index_map(index).
                            weight_map(weight).
                            distance_map(distance));

    dijkstra_shortest_paths_no_color_map(g, s,
                                         vertex_index_map(index).
                                         weight_map(weight).
                                         distance_map(distance));
  }
  {
    typedef incidence_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag> IncidenceGraph;
    typedef vertex_list_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag, IncidenceGraph> Graph;
    vertex_t s;
    typedef graph_traits<Graph>::edge_descriptor edge_t;
    readable_property_map_archetype<edge_t, std::size_t> weight;
    typedef property_graph_archetype<Graph, vertex_index_t, std::size_t>
      graph_t;
    graph_t& g = static_object<graph_t>::get();
    read_write_property_map_archetype<vertex_t, vertex_t> pred;
    dijkstra_shortest_paths(g, s,
                            predecessor_map(pred).
                            weight_map(weight));

    dijkstra_shortest_paths_no_color_map(g, s,
                                         predecessor_map(pred).
                                         weight_map(weight));
  }
  {
    typedef incidence_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag> IncidenceGraph;
    typedef vertex_list_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag, IncidenceGraph> Graph;
    vertex_t s;
    typedef property_graph_archetype<Graph, edge_weight_t, std::size_t>
      graph_t;
    graph_t& g = static_object<graph_t>::get();
    read_write_property_map_archetype<vertex_t, vertex_t> pred;
    readable_property_map_archetype<vertex_t, int> index;
    dijkstra_shortest_paths(g, s,
                            predecessor_map(pred).
                            vertex_index_map(index));

     dijkstra_shortest_paths_no_color_map(g, s,
                                          predecessor_map(pred).
                                          vertex_index_map(index));
  }
  {
    typedef incidence_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag> IncidenceGraph;
    typedef vertex_list_graph_archetype<vertex_t, directed_tag,
      allow_parallel_edge_tag, IncidenceGraph> graph_t;
    graph_t& g = static_object<graph_t>::get();
    vertex_t s;
    typedef graph_traits<graph_t>::edge_descriptor edge_t;
    readable_property_map_archetype<edge_t, dist_value> weight;
    readable_property_map_archetype<vertex_t, int> index;
    read_write_property_map_archetype<vertex_t, color_value_archetype> color;
    read_write_property_map_archetype<vertex_t, dist_value> distance;
    typedef binary_function_archetype<dist_value, dist_value, dist_value>
      Combine;
    Combine combine = static_object<Combine>::get();
    typedef binary_predicate_archetype<dist_value, dist_value>
      Compare;
    Compare compare = static_object<Compare>::get();
    dijkstra_visitor<> vis;

    dijkstra_shortest_paths(g, s, color_map(color).
                            vertex_index_map(index).
                            weight_map(weight).
                            distance_map(distance).
                            distance_combine(combine).
                            distance_compare(compare).
                            visitor(vis));

    dijkstra_shortest_paths_no_color_map(g, s,
                                         vertex_index_map(index).
                                         weight_map(weight).
                                         distance_map(distance).
                                         distance_combine(combine).
                                         distance_compare(compare).
                                         visitor(vis));
  }
  return 0;
}


#include <cstdlib>
#include <sysexits.h>
#include <iostream>
#include <string>
#include <type_traits>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/program_options.hpp>
#include <cvodes_anyode_parallel.hpp>

namespace po = boost::program_options;

${p_odesys_impl}

typedef ${p_realtype} realtype;
typedef ${p_indextype} indextype;

int main(int argc, char *argv[]){
    // Parse cmdline args:
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("atol", po::value<realtype>()->default_value(1e-8), "absolute tolerance")
        ("rtol", po::value<realtype>()->default_value(1e-8), "relative tolerance")
        ("mxsteps", po::value<int>()->default_value(500), "maximum number of steps")
        ("lmm", po::value<std::string>()->default_value("BDF"), "linear multistep method")
        ("return-on-root", po::value<bool>()->default_value(false), "Return on root")
        ("autorestart", po::value<int>()->default_value(0), "Autorestart (autonomous)")
        ("return-on-error", po::value<bool>()->default_value(false), "Return on error")
        ("with-jtimes", po::value<int>()->default_value(0), "With jtimes")
        ("get-dx-max-factor", po::value<realtype>()->default_value(1.0), "get_dx_max multiplicative factor")
        ("error-outside-bounds", po::value<bool>()->default_value(false), "Return recoverable error to solver when outside bounds")
        ("max-invariant-violation", po::value<realtype>()->default_value(0.0), "Limit at which to return recoverable error when supported by integrator.")
        ("special-settings", po::value<std::string>()->default_value(""), "special settings2 (if customized)")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    try {
        po::notify(vm);
    } catch (boost::exception& exc) {
        std::cerr << boost::diagnostic_information(exc);
    }

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

    const indextype ny = ${p_odesys.ny};
    const int nparams = ${len(p_odesys.params)};
    std::vector<odesys_anyode::OdeSys<realtype, indextype> *> systems;
    const std::vector<realtype> atol(ny, vm["atol"].as<realtype>());
    const realtype rtol(vm["rtol"].as<realtype>());
    const auto lmm((vm["lmm"].as<std::string>() == "adams") ?
                   cvodes_cxx::LMM::Adams :
                   cvodes_cxx::LMM::BDF);

    std::vector<realtype> y0;

    std::vector<realtype> tout;
    int nout = -1;
    const long int mxsteps(vm["mxsteps"].as<int>());

    std::vector<realtype> dx0;
    std::vector<realtype> dx_min;
    std::vector<realtype> dx_max;
    const bool with_jacobian = true;
    auto iter_type=cvodes_cxx::IterType::Undecided;
    int linear_solver=0;
    const int maxl=0;
    const realtype eps_lin=0.0;
    const unsigned nderiv=0;
    bool return_on_root(vm["return-on-root"].as<bool>());
    int autorestart(vm["autorestart"].as<int>());
    bool return_on_error(vm["return-on-error"].as<bool>());
    bool with_jtimes(vm["with-jtimes"].as<int>());

    std::vector<realtype> params;
    const realtype get_dx_max_factor(vm["get-dx-max-factor"].as<realtype>());
    bool error_outside_bounds = vm["error-outside-bounds"].as<bool>();
    const realtype max_invariant_violation(vm["max-invariant-violation"].as<realtype>());
    std::vector<realtype> special_settings;
    auto special_settings_stream = std::istringstream(vm["special-settings"].as<std::string>());
    for (std::string item; std::getline(special_settings_stream, item, ',');){
        special_settings.push_back(std::atof(item.c_str()));
    }
    // Parse stdin:
    int rowi=0;
    for (std::string line; std::getline(std::cin, line); ++rowi){
    // nt y0[0] ... y0[ny - 1] params[0] ... params[nparams - 1] t[0] ... t[nout - 1]
        if (line.size() == 0)
            break;
        std::string item;
        auto linestream = std::istringstream(line);

        std::getline(linestream, item, ' ');
        const int nt = std::atoi(item.c_str());

        for (indextype idx=0; idx<ny; ++idx){
            std::getline(linestream, item, ' ');
            y0.push_back(std::atof(item.c_str()));
        }

        for (int idx=0; idx<nparams; ++idx){
            std::getline(linestream, item, ' ');
            params.push_back(std::atof(item.c_str()));
        }

        std::getline(linestream, item, ' ');
        dx0.push_back(std::atof(item.c_str()));

        std::getline(linestream, item, ' ');
        dx_min.push_back(std::atof(item.c_str()));

        std::getline(linestream, item, ' ');
        dx_max.push_back(std::atof(item.c_str()));

        for (; std::getline(linestream, item, ' ');)
            tout.push_back(std::atof(item.c_str()));

        if (nout == -1){
            nout = tout.size();
        }
        if (tout.size() != (rowi+1)*nt or nt != nout){
            std::cerr << "Inconsistent length of tout" << std::endl;
            return 1;
        }

        systems.push_back(new odesys_anyode::OdeSys<realtype, indextype>(&params[systems.size()*nparams],
                                                                         atol, rtol, get_dx_max_factor,
                                                                         error_outside_bounds,
                                                                         max_invariant_violation,
                                                                         special_settings));
    }
    // Computations:
    int nprealloc = 500;
    int * td_arr;
    realtype ** xyout_arr;
    std::vector<std::pair<int, std::vector<int> > > n_ri;
    std::vector<std::pair<int, std::pair<std::vector<int>, std::vector<realtype> > > > ri_ro;
    std::vector<realtype> yout;

    if (nout < 2){
        std::cerr << "Got too few (" << nout << ") time points." << std::endl;
    } else if (nout == 2) {
        xyout_arr = (realtype **)malloc(systems.size()*sizeof(realtype*));
        for (int i=0; i<systems.size(); ++i) {
            xyout_arr[i] = (realtype*)malloc((ny+1)*nprealloc*sizeof(realtype));
            td_arr[i] = nprealloc;
        }
        std::vector<realtype> t0;
        std::vector<realtype> tend;
        for (int idx=0; idx<systems.size(); ++idx){
            tend.push_back(tout[2*idx + 1]);
            xyout_arr[idx][0] = tout[2*idx];
            for (indextype iy=0; iy<ny; ++iy){
                xyout_arr[idx][iy + 1] = y0[ny*idx + iy];
            }
        }
        n_ri = cvodes_anyode_parallel::multi_adaptive(
	    xyout_arr, td_arr, systems, atol, rtol, lmm, &tend[0], mxsteps, &dx0[0],
            &dx_min[0], &dx_max[0], with_jacobian, iter_type, linear_solver, maxl,
            eps_lin, nderiv, return_on_root, autorestart, return_on_error, with_jtimes
        );

    } else {
        yout.resize(systems.size()*ny*nout);
        ri_ro = cvodes_anyode_parallel::multi_predefined(
        systems, atol, rtol, lmm, &y0[0], nout, &tout[0], &yout[0], mxsteps, &dx0[0],
        &dx_min[0], &dx_max[0], with_jacobian, iter_type, linear_solver, maxl,
        eps_lin, nderiv, autorestart, return_on_error, with_jtimes);
    }
    // Output:
    for (indextype si=0; si<systems.size(); ++si){
        bool first = true;
        for (int pi=0; pi<nparams; ++pi){
            if (first)
                first = false;
            else
                std::cout << ' ';
            std::cout << params[si*nparams + pi];
        }
        std::cout << '\n';
        if (nout == 2) {
            for (int ti=0; ti <= n_ri[si].first; ++ti){
                std::cout << xyout_arr[si][(1+ny)*ti];
                for (indextype yi=0; yi<ny; ++yi)
                    std::cout << ' ' << xyout_arr[si][(1+ny)*ti+yi+1];
                std::cout << '\n';
            }
        } else {
            for (indextype ti=0; ti<nout; ++ti){
                std::cout << tout[si*nout + ti];
                for (indextype yi=0; yi<ny; ++yi){
                    std::cout << ' ' << yout[si*nout*ny + ti*ny + yi];
                }
                std::cout << '\n';
            }
        }
        std::cout << '{';
        first = true;
        for (const auto& itm : systems[si]->current_info->nfo_int) {
            if (first){
                first = false;
            } else {
                std::cout << ", ";
            }
            std::cout << "'" << itm.first << "': " << itm.second;
        }
        for (const auto& itm : systems[si]->current_info.nfo_dbl) {
            std::cout << ", '" << itm.first << "': " << itm.second;
        }
        for (const auto& itm : systems[si]->current_info.nfo_vecdbl) {
            std::cout << ", '" << itm.first << "': {";
            first = true;
            for (const auto& elem : itm.second) {
                if (first){
                    first = false;
                } else {
                    std::cout << ", ";
                }
                std::cout << elem;
            }
            std::cout << "}";
        }
        for (const auto& itm : systems[si]->current_info.nfo_vecint) {
            std::cout << ", '" << itm.first << "': {";
            first = true;
            for (const auto& elem : itm.second) {
                if (first){
                    first = false;
                } else {
                    std::cout << ", ";
                }
                std::cout << elem;
            }
            std::cout << "}";
        }

        std::cout << "}\n";
    }

    for (auto& v : systems)
        delete v;
    return EX_OK;
}


#include "Pose2D.h"

#include "AlignedBoundingBox3D.h"
#include "BoundingBox2D.h"
#include "Circle2D.h"
#include "Line2D.h"
#include "LineSegment.h"
#include "Polygon.h"
#include <boost/concept_check.hpp>

using namespace taco;
using namespace Eigen;

Pose2D::Pose2D() : _position(0, 0), _angle(0)
{
}

Pose2D::Pose2D(const Pose2D &other) : _position(other._position), _angle(other._angle)
{
}

Pose2D::Pose2D(const Vector2d &position) : _position(position), _angle(0)
{
}

Pose2D::Pose2D(const double &x, const double &y) : _position(x, y), _angle()
{
}

Pose2D::Pose2D(const Angle &angle) : _position(0, 0), _angle(angle)
{
}

Pose2D::Pose2D(const Vector2d &position, const Angle &angle) : _position(position), _angle(angle)
{
}

Pose2D::Pose2D(const double &x, const double &y, const Angle &angle) : _position(x, y), _angle(angle)
{
}

Pose2D::~Pose2D()
{
}

double Pose2D::x() const
{
	return _position(0);
}

double Pose2D::y() const
{
	return _position(1);
}

const Vector2d &Pose2D::getPosition() const
{
	return _position;
}

const Angle &Pose2D::getAngle() const
{
	return _angle;
}

Pose2D &Pose2D::operator=(const Pose2D &rhs)
{
	_position = rhs._position;
	_angle = rhs._angle;
	return *this;
}

Pose2D &Pose2D::operator*=(const Pose2D &rhs)
{
	_position += _angle * rhs._position;
	_angle += rhs._angle;
	return *this;
}

Pose2D &Pose2D::operator-=(const Pose2D &rhs)
{
	_position = rhs._angle / (_position - rhs._position);
	_angle -= rhs._angle;
	return *this;
}

Pose2D &Pose2D::operator/=(const Pose2D &rhs)
{
	_position = _angle / (rhs._position - _position);
	_angle = rhs._angle - _angle;
	return *this;
}

void Pose2D::setPosition(const Vector2d &position)
{
	_position = position;
}

void Pose2D::setPosition(const double &x, const double &y)
{
	_position(0) = x;
	_position(1) = y;
}

void Pose2D::setAngle(const Angle &angle)
{
	_angle = angle;
}

const Pose2D Pose2D::operator+(const Pose2D &other) const
{
	return Pose2D(getPosition() + other.getPosition(), getAngle() + other.getAngle());
}

const Pose2D Pose2D::operator*(const Pose2D &other) const
{
	return Pose2D(*this) *= other;
}

const Vector2d Pose2D::operator*(const Vector2d &vec) const
{
	return _position + _angle * vec;
}

const Line2D Pose2D::operator*(const Line2D &line) const
{
	return Line2D(*this * line.getStart(), *this * line.getEnd());
}

const Line2D Pose2D::operator/(const Line2D &line) const
{
	return Line2D(*this / line.getStart(), *this / line.getEnd());
}

const Circle2D Pose2D::operator*(const Circle2D &circle) const
{
	return Circle2D(*this * circle.origin(), circle.radius());
}

const Circle2D Pose2D::operator/(const Circle2D &circle) const
{
	return Circle2D(*this / circle.origin(), circle.radius());
}

const LineSegment Pose2D::operator*(const LineSegment &lineSegment) const
{
	return LineSegment(*this * lineSegment.getStartPose(), lineSegment.getLength(), lineSegment.getExitAngle());
}

const LineSegment Pose2D::operator/(const LineSegment &lineSegment) const
{
	return LineSegment(*this / lineSegment.getStartPose(), lineSegment.getLength(), lineSegment.getExitAngle());
}

const Polygon::Ptr Pose2D::operator*(Polygon::ConstPtr polygon) const
{
	if (!polygon) {
		return Polygon::Ptr();
	}

	std::vector<Vector2d> transformedPoints;
	for (Vector2d p : polygon->getPoints()) {
		transformedPoints.push_back(*this * p);
	}

	return boost::make_shared<Polygon>(transformedPoints);
}

const Polygon::Ptr Pose2D::operator/(Polygon::ConstPtr polygon) const
{
	if (!polygon) {
		return Polygon::Ptr();
	}

	std::vector<Vector2d> transformedPoints;
	for (Vector2d p : polygon->getPoints()) {
		transformedPoints.push_back(*this / p);
	}

	return boost::make_shared<Polygon>(transformedPoints);
}

const Pose2D Pose2D::operator-(const Pose2D &other) const
{
	return Pose2D(*this) -= other;
}

const Pose2D Pose2D::operator/(const Pose2D &other) const
{
	return Pose2D(*this) /= other;
}

const Vector2d Pose2D::operator/(const Vector2d &vec) const
{
	return _angle / (vec - _position);
}

const BoundingBox2D Pose2D::operator*(const BoundingBox2D &box) const
{
	return BoundingBox2D(Pose2D(*this) * box.pose(), *this * box.minPoint(), *this * box.maxPoint());
}

const BoundingBox2D Pose2D::operator/(const BoundingBox2D &box) const
{
	return BoundingBox2D(Pose2D(*this) / box.pose(), *this / box.minPoint(), *this / box.maxPoint());
}

const AlignedBoundingBox3D Pose2D::operator*(const AlignedBoundingBox3D &box) const
{
	Vector2d origin2D(box.origin()(0), box.origin()(1));
	Vector2d minPoint2D(box.minPoint()(0), box.minPoint()(1));
	Vector2d maxPoint2D(box.maxPoint()(0), box.maxPoint()(1));
	origin2D = *this * origin2D;
	minPoint2D = _angle * minPoint2D;
	maxPoint2D = _angle * maxPoint2D;
	Vector3d origin3D = Vector3d(origin2D(0), origin2D(1), box.origin()(2));
	Vector3d minPoint3D = Vector3d(minPoint2D(0), minPoint2D(1), box.minPoint()(2));
	Vector3d maxPoint3D = Vector3d(maxPoint2D(0), maxPoint2D(1), box.maxPoint()(2));

	return AlignedBoundingBox3D(origin3D, minPoint3D, maxPoint3D);
}

const AlignedBoundingBox3D Pose2D::operator/(const AlignedBoundingBox3D &box) const
{
	Vector2d origin2D(box.origin()(0), box.origin()(1));
	Vector2d minPoint2D(box.minPoint()(0), box.minPoint()(1));
	Vector2d maxPoint2D(box.maxPoint()(0), box.maxPoint()(1));
	origin2D = *this / origin2D;
	minPoint2D = _angle / minPoint2D;
	maxPoint2D = _angle / maxPoint2D;
	Vector3d origin3D = Vector3d(origin2D(0), origin2D(1), box.origin()(2));
	Vector3d minPoint3D = Vector3d(minPoint2D(0), minPoint2D(1), box.minPoint()(2));
	Vector3d maxPoint3D = Vector3d(maxPoint2D(0), maxPoint2D(1), box.maxPoint()(2));

	return AlignedBoundingBox3D(origin3D, minPoint3D, maxPoint3D);
}

bool Pose2D::operator==(const Pose2D &other) const
{
	return _position == other._position && _angle == other._angle;
}

bool Pose2D::operator!=(const Pose2D &other) const
{
	return !(*this == other);
}

const Pose2D Pose2D::invert() const
{
	Vector2d inversePos(-1 * (_angle * _position));

	return Pose2D(inversePos, _angle.negate());
}

const double Pose2D::getDistanceTo(const Pose2D &other) const
{
	double deltax = _position(0) - other._position(0);
	double deltay = _position(1) - other._position(1);
	return std::sqrt(deltax * deltax + deltay * deltay);
}

const Angle Pose2D::getDeltaAngle(const Pose2D &other) const
{
	return other._angle - _angle;
}

const Vector2d Pose2D::getUnitVector() const
{
	return Vector2d(std::cos(_angle.rad()), std::sin(_angle.rad()));
}

const Angle Pose2D::getAngleTo(const Pose2D &other) const
{
	return getAngleTo(other.getPosition());
}

const Angle Pose2D::getAngleTo(const Vector2d &position) const
{
	Vector2d relative = *this / position;
	return Angle::to(relative);
}

// ========== STATIC HELPER METHODS ===========
const Pose2D Pose2D::average(const Pose2D &p1, const Pose2D &p2, const double &weight1, const double &weight2)
{
	double divider = weight1 + weight2;
	double x = (p1.x() * weight1 + p2.x() * weight2) / divider;
	double y = (p1.y() * weight1 + p2.y() * weight2) / divider;

	return Pose2D(x, y, Angle::average(p1.getAngle(), p2.getAngle(), weight1, weight2));
}

const Pose2D Pose2D::average(const std::vector<Pose2D> &poses)
{
	size_t size = poses.size();
	if (size == 0) {
		return Pose2D();
	} else if (size == 1) {
		return poses[0];
	}

	double xSum = 0;
	double ySum = 0;
	double angleSum = 0;

	for (Pose2D p : poses) {
		xSum += p.x();
		ySum += p.y();
		angleSum += p.getAngle().rad() + M_PI;
	}

	return Pose2D(xSum / size, ySum / size, Angle::rad((angleSum / size) - M_PI));
}

const Pose2D Pose2D::interpolate(const Pose2D &startPose, const Pose2D &endPose, const double &progress)
{
	if (progress <= 0) {
		return startPose;
	} else if (progress >= 1) {
		return endPose;
	}

	Vector2d diff = endPose.getPosition() - startPose.getPosition();
	Angle angle = Angle::interpolate(startPose.getAngle(), endPose.getAngle(), progress);

	return Pose2D(startPose.getPosition() + (diff * progress), angle);
}

namespace taco
{
std::ostream &operator<<(std::ostream &os, const Pose2D &pose)
{
	os << pose.x() << " " << pose.y() << " " << pose.getAngle();
	return os;
}
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/c180.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check c180 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::c180;
  using boost::simd::C180;

  STF_TYPE_IS(decltype(C180<T>()), T);
  STF_EQUAL(C180<T>(), T(180));
  STF_EQUAL(c180( as(T{}) ),T(180));
}

STF_CASE_TPL( "Check c180 behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::c180;
  using boost::simd::C180;

  STF_TYPE_IS(decltype(C180<T>()), T);
  STF_IEEE_EQUAL(C180<T>(), T(180));
  STF_IEEE_EQUAL(c180( as(T{}) ), T(180));
}


//
// Copyright (c) 2011-2012 Krzysztof Jusiak (krzysztof at jusiak dot net)
//
// 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 QFSM_BACK_AUX_CHECKORTHOGONALITY_HPP
#define QFSM_BACK_AUX_CHECKORTHOGONALITY_HPP

#include <boost/mpl/fold.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/count.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/is_sequence.hpp>
#include "QFsm/Back/Aux/Flatten.hpp"
#include "QFsm/Back/Aux/TransitionTypes.hpp"

namespace QFsm
{
namespace Back
{
namespace Aux
{

/**
 * One state should belongs only to the one orthogonal region
 * BOOST_MPL_ASSERT((QFsm::Back::Aux::CheckOrthogonality<TInitialState, TTransitionTable>));
 */
template<typename TOrthogonality, typename TInitialState, typename TTransitionTable, bool assert = true>
class CheckOrthogonality
{
    BOOST_MPL_ASSERT((boost::mpl::is_sequence<TTransitionTable>));

    template<typename State, typename Set = boost::mpl::set0<> >
    struct GetRecursivelyTargetStates : boost::mpl::fold
        <
            TTransitionTable,
            Set,
            boost::mpl::if_
            <
                boost::is_same<Back::Aux::InitState<boost::mpl::_2>, State>,
                boost::mpl::if_
                <
                    boost::mpl::has_key<boost::mpl::_1, Back::Aux::TargetState<boost::mpl::_2> >,
                    boost::mpl::_1,
                    GetRecursivelyTargetStates
                    <
                        Back::Aux::TargetState<boost::mpl::_2>,
                        boost::mpl::insert<boost::mpl::_1, Back::Aux::TargetState<boost::mpl::_2> >
                    >
                >,
                boost::mpl::_1
            >
        >
    { };

    struct JoinAllRegionsStates : QFsm::Aux::Flatten
        <
            boost::mpl::fold
            <
                TInitialState,
                boost::mpl::vector0<>,
                boost::mpl::push_back<boost::mpl::_1, GetRecursivelyTargetStates<boost::mpl::_2> >
            >
        >
    { };

    typedef typename QFsm::Aux::Flatten
    <
        typename boost::mpl::fold
        <
            TInitialState,
            boost::mpl::vector0<>,
            boost::mpl::push_back<boost::mpl::_1, GetRecursivelyTargetStates<boost::mpl::_2> >
        >::type
    >::type RegionsStatesJoined;

    struct NotTrulyOrthogonalStates : boost::mpl::fold
        <
            RegionsStatesJoined,
            boost::mpl::set0<>,
            boost::mpl::if_
            <
                boost::mpl::greater< boost::mpl::count<RegionsStatesJoined, boost::mpl::_2>, boost::mpl::size_t<1> >,
                boost::mpl::insert<boost::mpl::_1, boost::mpl::_2>,
                boost::mpl::_1
            >
        >
    { };

public:
    typedef typename boost::mpl::empty<typename NotTrulyOrthogonalStates::type> type;

    //regions are truly orthogonal if each state belongs exactly to one orthogonal region
    //states which belongs to more than one orthogonal regions are listed by type
    BOOST_MPL_ASSERT_MSG((boost::mpl::or_<type, boost::mpl::bool_<!assert> >::value), REGIONS_ARE_NOT_TRULY_ORTHOGONAL, (typename NotTrulyOrthogonalStates::type));
};

template<typename TInitialState, typename TTransitionTable, bool assert>
class CheckOrthogonality<DontCheckOrthogonality, TInitialState, TTransitionTable, assert> : public boost::mpl::true_ { };

} // namespace Aux
} // namespace Back
} // namespace QFsm

#endif