#include <boost/circular_buffer.hpp>
#include <iostream>
#include <mutex>

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float64.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <pcl/filters/voxel_grid.h>

#include <pclomp/ndt_omp.h>

#include <hdl_localization/pose_estimator.hpp>

namespace hdl_localization {

class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;

  HdlLocalizationNodelet() {}
  virtual ~HdlLocalizationNodelet() {}

  void onInit() override {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    processing_time.resize(16);
    initialize_params();

    map_frame_id = private_nh.param<std::string>("map_frame_id", "world");
    odom_child_frame_id =
        private_nh.param<std::string>("odom_child_frame_id", "base_link");

    use_guesses = private_nh.param<bool>("use_guesses", true);
    if (use_guesses) {
      NODELET_INFO("enable guess-based prediction");
      guess_sub = mt_nh.subscribe(
          "/guess", 256, &HdlLocalizationNodelet::guess_callback, this);
    }
    points_sub = mt_nh.subscribe(
        "/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe(
        "/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe(
        "/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);

    pose_pub = nh.advertise<nav_msgs::Odometry>("/odom", 5, false);
    aligned_pub =
        nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 5, false);
    score_pub = nh.advertise<std_msgs::Float64>("/score", 5, false);
  }

private:
  void initialize_params() {
    // intialize scan matching method
    double downsample_resolution =
        private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method =
        private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(
        new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution,
                           downsample_resolution);
    downsample_filter = voxelgrid;

    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(
        new pclomp::NormalDistributionsTransform<PointT, PointT>());
    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if (ndt_neighbor_search_method == "DIRECT1") {
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
    } else if (ndt_neighbor_search_method == "DIRECT7") {
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
    } else {
      if (ndt_neighbor_search_method == "KDTREE") {
        NODELET_INFO("search_method KDTREE is selected");
      } else {
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
    }
    registration = ndt;

    // initialize pose estimator
    if (private_nh.param<bool>("specify_init_pose", true)) {
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(
          registration, ros::Time::now(),
          Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0),
                          private_nh.param<double>("init_pos_y", 0.0),
                          private_nh.param<double>("init_pos_z", 0.0)),
          Eigen::Quaternionf(private_nh.param<double>("init_ori_w", 1.0),
                             private_nh.param<double>("init_ori_x", 0.0),
                             private_nh.param<double>("init_ori_y", 0.0),
                             private_nh.param<double>("init_ori_z", 0.0)),
          private_nh.param<double>("cool_time_duration", 0.5)));
    }
  }

private:
  /**
   * @brief callback for guesses
   * @param pose
   */
  void
  guess_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr pose) {
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    auto p = pose->pose.pose.position;
    auto o = pose->pose.pose.orientation;
    auto stamp = pose->header.stamp;
    Eigen::Vector3f pos(p.x, p.y, p.z);
    Eigen::Vector4f ori(o.w, o.x, o.y, o.z);
    pose_estimator->predict(stamp, pos, ori);
  }

  /**
   * @brief callback for point cloud data
   * @param points_msg
   */
  void points_callback(const sensor_msgs::PointCloud2ConstPtr &points_msg) {
    std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
    if (!pose_estimator) {
      NODELET_ERROR("waiting for initial pose input!!");
      return;
    }

    if (!globalmap) {
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }

    const auto &stamp = points_msg->header.stamp;
    pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *pcl_cloud);

    if (pcl_cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }

    // transform pointcloud into odom_child_frame_id
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    if (!tf_listener.waitForTransform(odom_child_frame_id, "velodyne", stamp,
                                      ros::Duration(0.5))) {
      NODELET_WARN("timeout at waiting for transformer update!!");
      return;
    }

    if (!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud,
                                      tf_listener)) {
      NODELET_WARN("point cloud cannot be transformed into target frame!!");
      return;
    }

    // predict
    auto q = pose_estimator->quat();
    Eigen::Vector4f ori(q.w(), q.x(), q.y(), q.z());
    pose_estimator->predict(stamp,
                            pose_estimator->pos() + pose_estimator->vel(), ori);

    // correct
    auto filtered = downsample(cloud);
    auto t1 = ros::WallTime::now();
    auto aligned = pose_estimator->correct(filtered);
    auto t2 = ros::WallTime::now();
    score_pub.publish(pose_estimator->score);

    processing_time.push_back((t2 - t1).toSec());
    double avg_processing_time =
        std::accumulate(processing_time.begin(), processing_time.end(), 0.0) /
        processing_time.size();

    if (aligned_pub.getNumSubscribers()) {
      aligned->header.frame_id = map_frame_id;
      aligned->header.stamp = cloud->header.stamp;
      aligned_pub.publish(aligned);
    }

    publish_odometry(points_msg->header.stamp, pose_estimator->matrix());
  }

  /**
   * @brief callback for globalmap input
   * @param points_msg
   */
  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr &points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;

    registration->setInputTarget(globalmap);
  }

  /**
   * @brief callback for initial pose input ("2D Pose Estimate" on rviz)
   * @param pose_msg
   */
  void initialpose_callback(
      const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg) {
    NODELET_INFO("initial pose received!!");
    std::lock_guard<std::mutex> lock(pose_estimator_mutex);
    const auto &p = pose_msg->pose.pose.position;
    const auto &q = pose_msg->pose.pose.orientation;
    pose_estimator.reset(new hdl_localization::PoseEstimator(
        registration, ros::Time::now(), Eigen::Vector3f(p.x, p.y, p.z),
        Eigen::Quaternionf(q.w, q.x, q.y, q.z),
        private_nh.param<double>("cool_time_duration", 0.5)));
  }

  /**
   * @brief downsampling
   * @param cloud   input cloud
   * @return downsampled cloud
   */
  pcl::PointCloud<PointT>::ConstPtr
  downsample(const pcl::PointCloud<PointT>::ConstPtr &cloud) const {
    if (!downsample_filter) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
  }

  /**
   * @brief publish odometry
   * @param stamp  timestamp
   * @param pose   odometry pose to be published
   */
  void publish_odometry(const ros::Time &stamp, const Eigen::Matrix4f &pose) {
    // broadcast the transform over tf
    geometry_msgs::TransformStamped odom_trans =
        matrix2transform(stamp, pose, map_frame_id, odom_child_frame_id);
    pose_broadcaster.sendTransform(odom_trans);

    // publish the transform
    nav_msgs::Odometry odom;
    odom.header.stamp = stamp;
    odom.header.frame_id = map_frame_id;

    odom.pose.pose.position.x = pose(0, 3);
    odom.pose.pose.position.y = pose(1, 3);
    odom.pose.pose.position.z = pose(2, 3);
    odom.pose.pose.orientation = odom_trans.transform.rotation;

    odom.child_frame_id = odom_child_frame_id;
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    pose_pub.publish(odom);
  }

  /**
   * @brief convert a Eigen::Matrix to TransformedStamped
   * @param stamp           timestamp
   * @param pose            pose matrix
   * @param frame_id        frame_id
   * @param child_frame_id  child_frame_id
   * @return transform
   */
  geometry_msgs::TransformStamped
  matrix2transform(const ros::Time &stamp, const Eigen::Matrix4f &pose,
                   const std::string &frame_id,
                   const std::string &child_frame_id) {
    Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;

  std::string map_frame_id;
  std::string odom_child_frame_id;

  bool use_guesses;
  ros::Subscriber guess_sub;
  ros::Subscriber points_sub;
  ros::Subscriber globalmap_sub;
  ros::Subscriber initialpose_sub;

  ros::Publisher pose_pub;
  ros::Publisher aligned_pub;
  ros::Publisher score_pub;
  tf::TransformBroadcaster pose_broadcaster;
  tf::TransformListener tf_listener;

  // globalmap and registration method
  pcl::PointCloud<PointT>::Ptr globalmap;
  pcl::Filter<PointT>::Ptr downsample_filter;
  pcl::Registration<PointT, PointT>::Ptr registration;

  // pose estimator
  std::mutex pose_estimator_mutex;
  std::unique_ptr<hdl_localization::PoseEstimator> pose_estimator;

  // processing time buffer
  boost::circular_buffer<double> processing_time;
};

} // namespace hdl_localization

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet,
                       nodelet::Nodelet)


// This file is part of slideio project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://slideio.com/license.html.
#include "slideio/imagetools/imagetools.hpp"

#include <numeric>

#include "slideio/slideio.hpp"
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <opencv2/imgproc.hpp>

int slideio::ImageTools::dataTypeSize(slideio::DataType dt)
{
    switch(dt)
    {
    case DataType::DT_Byte:
    case DataType::DT_Int8:
        return 1;
    case DataType::DT_UInt16:
    case DataType::DT_Int16:
    case DataType::DT_Float16:
        return 2;
    case DataType::DT_Int32:
    case DataType::DT_Float32:
        return 4;
    case DataType::DT_Float64:
        return 8;
    case DataType::DT_Unknown:
    case DataType::DT_None:
        break;
    }
    throw std::runtime_error(
        (boost::format("Unknown data type: %1%") % (int)dt).str());
}

void slideio::ImageTools::scaleRect(const cv::Rect& srcRect, const cv::Size& newSize, cv::Rect& trgRect)
{
    double scaleX = static_cast<double>(newSize.width) / static_cast<double>(srcRect.width);
    double scaleY = static_cast<double>(newSize.height) / static_cast<double>(srcRect.height);
    trgRect.x = static_cast<int>(std::floor(static_cast<double>(srcRect.x)*scaleX));
    trgRect.y = static_cast<int>(std::floor(static_cast<double>(srcRect.y)*scaleY));
    trgRect.width = newSize.width;
    trgRect.height = newSize.height;
}

void slideio::ImageTools::scaleRect(const cv::Rect& srcRect, double scaleX, double scaleY, cv::Rect& trgRect)
{
    trgRect.x = static_cast<int>(std::floor(static_cast<double>(srcRect.x)*scaleX));
    trgRect.y = static_cast<int>(std::floor(static_cast<double>(srcRect.y)*scaleY));
    int xn = srcRect.x + srcRect.width;
    int yn = srcRect.y + srcRect.height;
    int dxn = static_cast<int>(std::ceil(static_cast<double>(xn)* scaleX));
    int dyn = static_cast<int>(std::ceil(static_cast<double>(yn)* scaleY));
    trgRect.width = dxn - trgRect.x;
    trgRect.height = dyn - trgRect.y;
}

double slideio::ImageTools::computeSimilarity(const cv::Mat& leftM, const cv::Mat& rightM)
{
    double similarity = 0;

    // convert to 8bit images
    cv::Mat oneChannelLeftM = leftM.reshape(1);
    cv::Mat oneChannelRightM = rightM.reshape(1);
    double minLeft(0), maxLeft(0), minRight(0), maxRight(0);
    cv::minMaxLoc(oneChannelLeftM, &minLeft, &maxLeft);
    cv::minMaxLoc(oneChannelRightM, &minRight, &maxRight);
    double minVal = std::min(minLeft, minRight);
    double maxVal = std::max(maxLeft, maxRight);
    double absVal = maxVal;
    absVal -= minVal;
    double alpha = 255. / absVal;
    double beta = -minVal * alpha;

    cv::Mat left, right;
    leftM.convertTo(left, CV_MAKE_TYPE(CV_8U, left.channels()), alpha, beta);
    rightM.convertTo(right, CV_MAKE_TYPE(CV_8U, right.channels()), alpha, beta);

    cv::Rect rectWindow(0, 0, 30, 30);
    const int width = left.size[1];
    const int height = left.size[0];
    std::vector<double> scores;
    cv::Rect image(0, 0, width, height);
    const int binCount = 10;

    for (int y = 0; y < height; y += rectWindow.height)
    {
        for (int x = 0; x < width; x += rectWindow.width)
        {
            cv::Rect rectWnd2(rectWindow);
            rectWnd2.x = x;
            rectWnd2.y = y;
            cv::Rect rectRoi = rectWnd2 & image;
            cv::Mat leftRoi = left(rectRoi);
            cv::Mat rightRoi = right(rectRoi);
            double score = compareHistograms(leftRoi, rightRoi, binCount);
            scores.push_back(score);
        }
    }
    similarity = std::accumulate(scores.begin(), scores.end(), 0.0) / scores.size();
    return similarity;
}

double slideio::ImageTools::compareHistograms(const cv::Mat& left, const cv::Mat& right, int binCount)
{
    double similarity = 0;

    const int channelCount = left.channels();
    std::vector<int> channels(channelCount);

    std::vector<float*> ranges(channelCount);
    std::vector<int> histSizes(channelCount);
    std::vector<float> rangeVals(channelCount * 2);
    float minVal = 0;
    float maxVal = 256;

    for (int channel = 0; channel < channelCount; ++channel) {
        channels[channel] = channel;
        histSizes[channel] = binCount;
        const int channel2 = channel * 2;
        rangeVals[channel2] = (float)minVal;
        rangeVals[channel2 + 1] = (float)maxVal;
        ranges[channel] = &rangeVals[channel2];
    }

    cv::Mat histLeft, histRight;
    cv::Mat mask;

    cv::calcHist(&left, 1, channels.data(), mask, histLeft,
        channelCount, histSizes.data(), (const float**)ranges.data());

    cv::calcHist(&right, 1, channels.data(), mask, histRight,
        channelCount, histSizes.data(), (const float**)ranges.data());

    similarity = cv::compareHist(histLeft, histRight, cv::HISTCMP_CORREL);

    return similarity;
}


#include <iostream>
#include <exception>

#include <boost/filesystem.hpp>

#include "SUEPTree/Utils/interface/JECCorrector.h"

using namespace suep;

namespace {

  // Used to initialize the JetCorrectionParameters
  std::vector<JetCorrectorParameters> load_params (const std::string& files_base, const std::string& jet_type) {

    std::vector<JetCorrectorParameters> output {};

    const std::vector<std::string> levels = {
      "L1FastJet",
      "L2Relative",
      "L3Absolute",
      "L2L3Residual"
    };

    for (const auto& level : levels) {

      const std::string correction_file = files_base + "_" + level + "_" + jet_type + ".txt";

      if (not boost::filesystem::exists(correction_file)) {
        std::cerr << "Cannot find correction file: " << correction_file << std::endl;
        throw std::runtime_error {"No file"};
      }

      output.emplace_back(correction_file);
    }

    return output;

  }

}

JECCorrector::JECCorrector (const std::string& files_base, const std::string& jet_type) :
  m_corrector_params {load_params(files_base, jet_type)},
  m_corrector {m_corrector_params} {}

void JECCorrector::update_event (const suep::Event& event, const suep::JetCollection& jets, const RecoMet& met) {

  // Copy jets over
  m_corrected_jets = jets;
  m_corrected_met = met;

  TVector2 new_met {met.v()};
  TVector2 met_correction {};

  for (auto& jet : m_corrected_jets) {

    // FactorizedJetCorrectorCalculator is stupid and resets these every time
    m_corrector.setNPV(event.npv);
    m_corrector.setRho(event.rho);

    m_corrector.setJetEta(jet.eta());
    m_corrector.setJetPt(jet.rawPt);
    m_corrector.setJetE(jet.e());
    m_corrector.setJetPhi(jet.phi());
    m_corrector.setJetEMF(jet.cef + jet.nef); // Check that this is right
    m_corrector.setJetA(jet.area);

    auto scale = m_corrector.getCorrection();

    auto new_pt = scale * jet.rawPt;

    // Use minus new so that we don't have to flip phi.
    // This takes out the old correction and adds the new correction back in
    met_correction.SetMagPhi(jet.pt() - new_pt, jet.phi());

    new_met += met_correction;

    jet.setPtEtaPhiM(new_pt, jet.eta(), jet.phi(), jet.m());
  }

  m_corrected_jets.sort(suep::Particle::PtGreater);

  // Change stored MET
  m_corrected_met.setXY(new_met.X(), new_met.Y());

}

const JetCollection& JECCorrector::get_jets () const {

  return m_corrected_jets;

}

const RecoMet& JECCorrector::get_met () const {

  return m_corrected_met;

}


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MAKE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MAKE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/meta/hierarchy/logical.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_arithmetic.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <initializer_list>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  //------------------------------------------------------------------------------------------------
  // Some shared code between two specialisation
  template<typename Target, typename... Values>
  struct make_helper
  {
    using storage_t = typename Target::storage_type;

    static inline storage_t do_ ( Values const&... vs
                                , ::boost::simd::scalar_storage const&
                                ) BOOST_NOEXCEPT
    {
      storage_t that{{ static_cast<typename Target::substorage_type>(vs)... }};
      return that;
    }

    static inline Target do_( Values const&... vs
                            , ::boost::simd::aggregate_storage const&
                            ) BOOST_NOEXCEPT
    {
      using value_type = typename Target::value_type;
      std::initializer_list<value_type> lst{ static_cast<value_type>(vs)... };
      return load<Target>(lst.begin());
    }
  };

  template<typename Target, typename... Values>
  struct make_logical_helper
  {
    using storage_t = typename Target::storage_type;

    static inline Target do_( Values const&... vs
                            , ::boost::simd::aggregate_storage const&
                            ) BOOST_NOEXCEPT
    {
      using value_type = typename Target::value_type;
      std::initializer_list<value_type> lst{ static_cast<value_type>(vs)... };
      return load<Target>(lst.begin());
    }

    template<typename K>
    static inline Target do_( Values const&... vs
                            , K const&
                            ) BOOST_NOEXCEPT
    {
      using   value_t = typename as_arithmetic_t<Target>::value_type;
      return  bitwise_cast<Target>( make<as_arithmetic_t<Target>>( genmask<value_t>(vs)...));
    }
  };

  //------------------------------------------------------------------------------------------------
  // make from unspecified value
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values)
                          , bd::cpu_
                          , bd::target_<bs::pack_<bd::unspecified_<Target>,bs::simd_emulation_>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;
    using storage_t = typename target_t::storage_type;

    static_assert ( sizeof...(Values) == target_t::static_size
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return make_helper<target_t,Values...>::do_( vs..., typename target_t::storage_kind{});
    }
  };

  //------------------------------------------------------------------------------------------------
  // make from logical value in emulation mode - This is not duplicated code
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values, typename Ext)
                          , bd::cpu_
                          , bd::target_<bs::pack_<bs::logical_<Target>,Ext>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;
    using storage_t = typename target_t::storage_type;

    static_assert ( sizeof...(Values) == target_t::static_size
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return make_logical_helper<target_t,Values...>::do_(vs..., typename target_t::storage_kind{});
    }
  };
} } }

#endif


#include <algorithm>
#include <cassert>

#include <ros/package.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <boost/assign/list_of.hpp>
#include <boost/filesystem.hpp>

#include <anchoring/attribute.hpp>

namespace anchoring {

  // Set a global threashold for max spikes in distributions
  const double CONST_MAX_TH = 0.65;

  // Used namespace(s)
  using namespace std;

  // --[ Namspace functions ]--
  void sortAttribute( vector<string>  &symbols,
		      vector<float> &predictions,
		      int n ) {
    for( uint i = 0; i < predictions.size() - 1; i++ ) {
      for( uint j = i + 1; j < predictions.size(); j++ ) {
	if( predictions[i] < predictions[j] ) {
	  swapValues<float>( predictions[i], predictions[j]);
	  swapValues<std::string>( symbols[i], symbols[j]);
	}
      }
    }
    if( n >= 0 && n < (int)symbols.size() ) {
      symbols.erase( symbols.begin() + n, symbols.end());
      predictions.erase( predictions.begin() + n, predictions.end());
    }
  }

  // Mapping between attribute type and string
  AttributeType mapAttributeType(const string &type) {
    if( type == "category" ) return CATEGORY;
    if( type == "color" ) return COLOR;
    if( type == "descriptor" ) return DESCRIPTOR;
    if( type == "position" ) return POSITION;
    return SIZE;
  }

  // Create an attribute (pointer) based on the attribute type
  AttributePtr createAttribute(AttributeType type) {
    switch(type) {
    case CATEGORY:   return AttributePtr( new CategoryAttribute(type) );
    case COLOR:      return AttributePtr( new ColorAttribute(type) );
    case DESCRIPTOR: return AttributePtr( new DescriptorAttribute(type) );
    case POSITION:   return AttributePtr( new PositionAttribute(type) );
    default:         return AttributePtr( new SizeAttribute(type) );
    };
  }

  // ---------------------------------------
  // 0. Common attribute base struct methods
  // ------------------------------------------
  mongo::Database::Document AttributeCommon::serialize() {
    mongo::Database::Document doc;

    // Save the symbols
    try {
      doc.add<string>( "symbols", this->_symbols);
    }
    catch( const std::exception &e) {
      cout << "[AttributeCommon::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void AttributeCommon::deserialize(const mongo::Database::Document &doc) {

    // Load the symbols
    try {
      doc.get<string>( "symbols", this->_symbols);
    }
    catch( const std::exception &e) {
      cout << "[AttributeCommon::deserialize]" << e.what() << endl;
    }
  }

  // Symbol mapping of the attribute type
  string AttributeCommon::getTypeStr() {
    string result;
    switch(this->_type) {
    case CATEGORY:   result = "category"; break;
    case COLOR:      result = "color"; break;
    case DESCRIPTOR: result = "descriptor"; break;
    case POSITION:   result = "position"; break;
    case SIZE :      result = "size"; break;
    default:         result = "none"; break;
    }
    return result;
  }

  // -------------------------------
  // 1. Category attribute class methods
  // --------------_--------------------
  CategoryAttribute::CategoryAttribute( const anchor_msgs::CategoryAttribute &msg,
					AttributeType type ) : AttributeCommon( msg.symbols, type) {
    this->_predictions = vector<double>( msg.predictions.begin(), msg.predictions.end() );
    this->_n = 1.0;   // ...counter
  }

  mongo::Database::Document CategoryAttribute::serialize() {

    // Save the symbol(s)
    mongo::Database::Document doc = AttributeCommon::serialize();

    // Save (category) predictions
    try {
      doc.add<double>( "predictions", this->_predictions);
      doc.add<double>( "n", this->_n);  // ..including the frequency
    }
    catch( const std::exception &e) {
      cout << "[CategoryAttribute::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void CategoryAttribute::deserialize(const mongo::Database::Document &doc) {

    // Load (category) predictions
    try {
      doc.get<double>( "predictions", this->_predictions);
      this->_n = (float)doc.get<double>("n");  // ...including the frequency
    }
    catch( const std::exception &e) {
      cout << "[CategoryAttribute::deserialize]" << e.what() << endl;
    }

    // Load the symbol(s)
    AttributeCommon::deserialize(doc);
  }

  void CategoryAttribute::populate(anchor_msgs::Anchor &msg) {
    anchor_msgs::CategoryAttribute category_msg;
    //double max = *std::max_element( this->_predictions.begin(), this->_predictions.end());
    for( uint i = 0; i < this->_predictions.size(); i++ ) {
      //if( (this->_predictions[i] / max) > 0.25 ) {  // ...looking for spikes above max theshold value
      category_msg.symbols.push_back(this->_symbols[i]);
      category_msg.predictions.push_back((float)this->_predictions[i] / this->_n);
      //}
    }
    double max = *std::max_element( this->_predictions.begin(), this->_predictions.end()) / this->_n;
    sortAttribute( category_msg.symbols, category_msg.predictions, (max > 0.95 ? 1 : max > 0.65 ? 2 : 3 ));

    /*
    category_msg.symbols = this->_symbols;
    auto n = this->_N.begin();
    for( auto ite = this->_predictions.begin(); ite != this->_predictions.end(); ++ite, ++n) {
      categoryu_msg.predictions.push_back((float)*ite / *n);
    }
    */
    msg.category = category_msg;
  }

  void CategoryAttribute::populate(anchor_msgs::Display &msg) {
    float best = 0.0;
    int index = -1;
    for( uint i = 0; i < this->_symbols.size(); i++ ) {
      float prob = this->_predictions[i] / this->_n;
      if( prob > best ) {
	best = prob;
	index = i;
      }
    }
    if( index >= 0 ) {
      msg.category = this->_symbols[index];
      msg.prediction = this->_predictions[index] / this->_n;
    }

  }

  float CategoryAttribute::match(const AttributePtr &query_ptr) {

    // Typecast the query attribute pointer
    CategoryAttribute *raw_ptr = dynamic_cast<CategoryAttribute*>(query_ptr.get());
    assert( raw_ptr != nullptr );

    float result = 0.0;
    for( uint i = 0; i < this->_symbols.size(); i++ ) {
      for( uint j = 0; j < raw_ptr->_symbols.size(); j++ ) {
	if( this->_symbols[i] == raw_ptr->_symbols[j] && (this->_predictions[i] / this->_n) > 0.05 && raw_ptr->_predictions[j] > 0.05  ) {
	  float diff = 1.0 /
	    exp( abs( (this->_predictions[i] / this->_n) - raw_ptr->_predictions[j] ) /
		 ( (this->_predictions[i] / this->_n) + raw_ptr->_predictions[j] ) );
	  if (diff > result ) {
	    result = diff;
	  }
	}
      }
    }
    return result;
  }

  bool CategoryAttribute::update(const unique_ptr<AttributeCommon> &new_ptr) {

    // Typecast the new attribute pointer
    CategoryAttribute *raw_ptr = dynamic_cast<CategoryAttribute*>(new_ptr.get());
    assert( raw_ptr != nullptr );

    // Increment the counter
    this->_n = this->_n + raw_ptr->_n;

    // Summarize the predictions
    for( uint i = 0; i < this->_symbols.size(); i++ ) {
      for( uint j = 0; j < raw_ptr->_symbols.size(); j++ ) {
	if( this->_symbols[i] == raw_ptr->_symbols[j] ) {
	  this->_predictions[i] += raw_ptr->_predictions[j];
	}
      }
    }
    return true;
  }

  string CategoryAttribute::toString() {
    float best = 0.0;
    int index = -1;
    std::stringstream ss;
    for( uint i = 0; i < this->_symbols.size(); i++ ) {
      float prob = this->_predictions[i] / this->_n;
      if( prob > best ) {
	best = prob;
	index = i;
      }
    }
    if( index >= 0 ) {
      ss << this->_symbols[index];
    }
    return ss.str();
  }

  // --------------------------------
  // 2. Color attribute methods
  // ------------------------------------
  ColorAttribute::ColorAttribute( const anchor_msgs::ColorAttribute &msg,
				  AttributeType type ) : AttributeCommon( msg.symbols, type) {
    // Initilize counter
    this->_n = 1.0;

    // Read the data from ROS msg
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy( msg.data,
				    sensor_msgs::image_encodings::TYPE_32FC1 );
      cv_ptr->image.copyTo(this->_data);
    } catch (cv_bridge::Exception& e) {
      throw std::logic_error("[ColorAttribute::ColorAttribute]:" + std::string(e.what()) );
    }
    this->_predictions = vector<double>( msg.predictions.begin(), msg.predictions.end() );
  }

  mongo::Database::Document ColorAttribute::serialize() {

    // Save the symbol(s)
    mongo::Database::Document doc = AttributeCommon::serialize();

    // Save the color
    try {
      vector<double> array;
      for( int i = 0; i < this->_data.cols; i++) {
	array.push_back(this->_data.at<float>( 0, i));
      }
      doc.add<double>( "data", array);

      // Save the counter
      doc.add<double>( "n", this->_n);

      // Save (color) predictions
      doc.add<double>( "predictions", this->_predictions);
    }
    catch( const std::exception &e) {
      cout << "[ColorAttribute::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void ColorAttribute::deserialize(const mongo::Database::Document &doc) {

    // Load the color
    try {

      vector<double> array;
      doc.get<double>( "data", array);
      this->_data = cv::Mat( 1, array.size(), CV_32FC1);
      for( int i = 0; i < array.size(); i++) {
	this->_data.at<float>( 0, i) = (float)array[i];
      }

      // Load the counter
      this->_n = (float)doc.get<double>("n");

      // Load (color) predictions
      doc.get<double>( "predictions", this->_predictions);
    }
    catch( const std::exception &e) {
      cout << "[ColorAttribute::deserialize]" << e.what() << endl;
    }

    // Load the symbol(s)
    AttributeCommon::deserialize(doc);
  }

  void ColorAttribute::populate(anchor_msgs::Anchor &msg) {
    anchor_msgs::ColorAttribute color_msg;
    double max = *std::max_element( this->_predictions.begin(), this->_predictions.end());
    for( uint i = 0; i < this->_predictions.size(); i++ ) {
      if( (this->_predictions[i] / max) > CONST_MAX_TH ) {  // ...looking for spikes above max theshold value
	color_msg.symbols.push_back(this->_symbols[i]);
	color_msg.predictions.push_back((float)this->_predictions[i] / this->_n);
      }
    }
    sortAttribute( color_msg.symbols, color_msg.predictions, 5);
    msg.color = color_msg;
  }
  void ColorAttribute::populate(anchor_msgs::Display &msg) {
    double max = *std::max_element( this->_predictions.begin(), this->_predictions.end());
    for( uint i = 0; i < this->_predictions.size(); i++ ) {
      if( (this->_predictions[i] / max) > CONST_MAX_TH ) {  // ...looking for spikes above max theshold value
	msg.colors.push_back(this->_symbols[i]);
      }
    }
    // msg.colors = this->_symbols;
  }

  float ColorAttribute::match(const AttributePtr &query_ptr) {

    // Typecast the query attribute pointer
    ColorAttribute *raw_ptr = dynamic_cast<ColorAttribute*>(query_ptr.get());
    assert( raw_ptr != nullptr );

    float dist = (1.0 + cv::compareHist( raw_ptr->_data, (this->_data / this->_n), CV_COMP_CORREL)) / 2.0; // CV_COMP_CORREL | CV_COMP_INTERSECT | CV_COMP_BHATTACHARYYA
    //std::cout << "Color dist: " << dist << std::endl;
    return dist;
  }

  bool ColorAttribute::update(const AttributePtr &new_ptr) {

    // Typecast the new attribute pointer
    ColorAttribute *raw_ptr = dynamic_cast<ColorAttribute*>(new_ptr.get());
    assert( raw_ptr != nullptr );

    // Summarize histograms
    this->_data = this->_data + raw_ptr->_data;

    // Increment the counter
    this->_n = this->_n + raw_ptr->_n;

    // Summarize the predictions
    for( uint i = 0; i < this->_predictions.size(); i++ ) {
      this->_predictions[i] += raw_ptr->_predictions[i];
    }

    return true;
  }

  string ColorAttribute::toString() {
    std::stringstream ss;
    ss << "[ ";
    for( size_t i = 0; i < _symbols.size(); i++ ) {
      ss << _symbols[i];
      if( i + 1 < _symbols.size() ) {
	ss << ", ";
      }
    }
    ss << "]";
    return ss.str();
  }

  // ------------------------------------
  // 3. Descriptor attribute methods
  // --------------------------------------------
  DescriptorAttribute::DescriptorAttribute( const anchor_msgs::DescriptorAttribute &msg,
					    AttributeType type ) : AttributeCommon( msg.symbols, type) {
    // Read the data from ROS msg
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy( msg.data,
				    sensor_msgs::image_encodings::MONO8 );
      // (Deep) copy of the data
      cv_ptr->image.copyTo(this->_data);
    } catch (cv_bridge::Exception& e) {
      throw std::logic_error("[DescriptorAttribute::DescriptorAttribute]:" + std::string(e.what()) );
    }
  }

  mongo::Database::Document DescriptorAttribute::serialize() {

    // Save the symbol(s)
    mongo::Database::Document doc = AttributeCommon::serialize();

    // Save the descriptor
    try {
      doc.add<int>( "rows", (int)this->_data.rows);
      doc.add<int>( "cols", (int)this->_data.cols);

      std::size_t length = this->_data.rows * this->_data.cols;
      if( length > 0 ) {
	doc.add<unsigned char*>( "data", this->_data.data, length);
      }
    }
    catch( const std::exception &e) {
      cout << "[DescriptorAttribute::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void DescriptorAttribute::deserialize(const mongo::Database::Document &doc) {

    // Load the descriptor
    try {
      int rows = doc.get<int>( "rows");
      int cols = doc.get<int>( "cols");
      if( rows > 0 ) {
	unsigned char* array = doc.get<unsigned char*>( "data");
	this->_data = cv::Mat( rows, cols, CV_8U, array);
      }
    }
    catch( const std::exception &e) {
      cout << "[DescriptorAttribute::deserialize]" << e.what() << endl;
    }

    // Load the symbol(s)
    AttributeCommon::deserialize(doc);
  }

  // ----------------------------------
  // 4. Position attribute methods
  // ---------------------------------------
  PositionAttribute::PositionAttribute( const anchor_msgs::PositionAttribute &msg,
					AttributeType type ) : AttributeCommon( msg.symbols, type) {
    // Add the data (including a timestamp)
    this->_array.push_back(msg.data);
  }

  PositionAttribute::PositionAttribute( const vector<geometry_msgs::PoseStamped> &array,
					const vector<string> &symbols,
					AttributeType type ) : AttributeCommon( symbols, type) {
      // Add the data (including a timestamp)
    for( auto &pos : array ) {
      this->_array.push_back(pos);
    }
  }

  PositionAttribute::PositionAttribute( const AttributePtr &ptr,
					AttributeType type) : AttributeCommon(type) {

    // Typecast the query pointer
    PositionAttribute *raw_ptr = dynamic_cast<PositionAttribute*>(ptr.get());
    assert( raw_ptr != nullptr );

    // Add the data (including a timestamp)
    for( auto &pos : raw_ptr->_array ) {
      this->_array.push_back(pos);
    }
  }

  mongo::Database::Document PositionAttribute::serialize() {

    // Save the symbol(s)
    mongo::Database::Document doc = AttributeCommon::serialize();

    // Save the location
    try {

      vector<geometry_msgs::PoseStamped>::iterator ite_d = this->_array.begin();
      for( ; ite_d != this->_array.end(); ++ite_d ) {

	mongo::Database::Document subdoc;

	// Add time
	subdoc.add<double>( "t", ite_d->header.stamp.toSec());

	// Add position
	mongo::Database::Document position;
	position.add<double>( "x", ite_d->pose.position.x);
	position.add<double>( "y", ite_d->pose.position.y);
	position.add<double>( "z", ite_d->pose.position.z);
	subdoc.add( "position", position);

	// Add orientation
	mongo::Database::Document orientation;
	orientation.add<double>( "x", ite_d->pose.orientation.x);
	orientation.add<double>( "y", ite_d->pose.orientation.y);
	orientation.add<double>( "z", ite_d->pose.orientation.z);
	orientation.add<double>( "w", ite_d->pose.orientation.w);
	subdoc.add( "orientation", orientation);

	doc.append( "array", subdoc);
      }
    }
    catch( const std::exception &e) {
      cout << "[PositionAttribute::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void PositionAttribute::deserialize(const mongo::Database::Document &doc) {

    // Load the positions
    try {

      // Read a series of position data
      for( mongo::document_iterator ite = doc.begin("array"); ite != doc.end("array"); ++ite) {
	geometry_msgs::PoseStamped data;

	// Read the time
	data.header.stamp = ros::Time(ite->get<double>("t"));

	// Read position
	mongo::Database::Document position = ite->get("position");
	data.pose.position.x = position.get<double>("x");
	data.pose.position.y = position.get<double>("y");
	data.pose.position.z = position.get<double>("z");

	// Read orientation
	mongo::Database::Document orientation = ite->get("orientation");
	data.pose.orientation.x = orientation.get<double>("x");
	data.pose.orientation.y = orientation.get<double>("y");
	data.pose.orientation.z = orientation.get<double>("z");
	data.pose.orientation.w = orientation.get<double>("w");

	this->_array.push_back(data);
      }
    }
    catch( const std::exception &e) {
      cout << "[PositionAttribute::deserialize]" << e.what() << endl;
    }

    // Load the symbol(s)
    AttributeCommon::deserialize(doc);
  }

  // Overleaded functions for populating ROS messages
  void PositionAttribute::populate(anchor_msgs::Anchor &msg) {
    //anchor_msgs::PositionAttribute poistion;
    if( !this->_array.empty() ) {
      for( const auto &pos : this->_array ) {
        msg.position.data.pose.position.x += pos.pose.position.x;
        msg.position.data.pose.position.y += pos.pose.position.y;
        msg.position.data.pose.position.z += pos.pose.position.z;
        msg.position.data.pose.orientation.x += pos.pose.orientation.x;
        msg.position.data.pose.orientation.y += pos.pose.orientation.y;
        msg.position.data.pose.orientation.z += pos.pose.orientation.z;
        msg.position.data.pose.orientation.w += pos.pose.orientation.w;
      }
      msg.position.data.pose.position.x /= (int)this->_array.size();
      msg.position.data.pose.position.y /= (int)this->_array.size();
      msg.position.data.pose.position.z /= (int)this->_array.size();
      msg.position.data.pose.orientation.x /= (int)this->_array.size();
      msg.position.data.pose.orientation.y /= (int)this->_array.size();
      msg.position.data.pose.orientation.z /= (int)this->_array.size();
      msg.position.data.pose.orientation.w /= (int)this->_array.size();
    }
    //msg.position = poistion;
  }
  void PositionAttribute::populate(anchor_msgs::Display &msg) {
    if( !this->_array.empty() ) {
      for( const auto &pos : this->_array ) {
	msg.pos.pose.position.x += pos.pose.position.x;
	msg.pos.pose.position.y += pos.pose.position.y;
	msg.pos.pose.position.z += pos.pose.position.z;
      }
      msg.pos.pose.position.x /= (int)this->_array.size();
      msg.pos.pose.position.y /= (int)this->_array.size();
      msg.pos.pose.position.z /= (int)this->_array.size();
    }
  }

  float PositionAttribute::match(const AttributePtr &query_ptr) {

    // Typecast the query attribute pointer
    PositionAttribute *raw_ptr = dynamic_cast<PositionAttribute*>(query_ptr.get());
    assert( raw_ptr != nullptr );

    // The normalized L2 distance
    double _x, _y, _z, _d, _dist = -1.0;
    for( uint i = 0; i < this->_array.size(); i++ ) {
      geometry_msgs::PoseStamped train = this->_array[i];
      for( uint j = 0; j < raw_ptr->_array.size(); j++ ) {
	geometry_msgs::PoseStamped query = raw_ptr->_array[j];
	_x = query.pose.position.x - train.pose.position.x;
	_y = query.pose.position.y - train.pose.position.y;
	_z = query.pose.position.z - train.pose.position.z;
	_d  = sqrt( _x*_x + _y*_y + _z*_z );
	if( _dist < 0.0 || _d < _dist ) {
	  _dist = _d;
	}
      }
    }
    if( _dist >= 0.0 ) {
      return 1.0 / exp(_dist);
    }
    return 0.0;
  }

  bool PositionAttribute::update(const unique_ptr<AttributeCommon> &new_ptr) {

    // Typecast the new attribute pointer
    PositionAttribute *raw_ptr = dynamic_cast<PositionAttribute*>(new_ptr.get());
    assert( raw_ptr != nullptr );

    // Update the location
    this->_array.clear();
    for( auto &pos : raw_ptr->_array ) {
      this->_array.push_back(pos);
    }

    /*
    // Update the location
    if( this->match(new_ptr) < 0.01 ) { // ...not moved more than 1cm
      this->_array.back().header.stamp = raw_ptr->_array.back().header.stamp;
      // this->_array.back() = raw_ptr->_array.front();
      }
    else {
      // Append the location (including a timestamp)
      this->_array.push_back(raw_ptr->_array.back());
    }
    */

    /*
    // Append the symbol (if there exists an symbol)
    if( !raw_ptr->_symbols.empty() ) {
      this->_symbols.back() = raw_ptr->_symbols.front();
      this->_symbols.push_back(raw_ptr->_symbols.front());
    }
    */

    return true;
  }

  string PositionAttribute::toString() {
    std::stringstream ss;
    ss << "@ {";
    ss << " x = " << this->_array.back().pose.position.x;
    ss << ", y = " << this->_array.back().pose.position.y;
    ss << ", z = " << this->_array.back().pose.position.z;
    ss << "}";
    return ss.str();
  }

  // --------------------------------
  // 5. Size attribute class methods
  // -------------------------------------
  SizeAttribute::SizeAttribute( const anchor_msgs::SizeAttribute &msg,
				AttributeType type ) : AttributeCommon( msg.symbols, type) {
    this->_data = msg.data;
  }

  mongo::Database::Document SizeAttribute::serialize() {

    // Save the symbol(s)
    mongo::Database::Document doc = AttributeCommon::serialize();

    // Save the shape
    try {
      std::vector<double> size;
      size.push_back(this->_data.x);
      size.push_back(this->_data.y);
      size.push_back(this->_data.z);
      doc.add<double>( "data", size);
    }
    catch( const std::exception &e) {
      cout << "[SizeAttribute::serialize]" << e.what() << endl;
    }
    return doc;
  }

  void SizeAttribute::deserialize(const mongo::Database::Document &doc) {

    // Load the shape
    try {
      vector<double> size;
      doc.get<double>( "data", size);
      this->_data.x = size[0];
      this->_data.y = size[1];
      this->_data.z = size[2];
    }
    catch( const std::exception &e) {
      cout << "[SizeAttribute::deserialize]" << e.what() << endl;
    }

    // Load the symbol(s)
    AttributeCommon::deserialize(doc);
  }

  void SizeAttribute::populate(anchor_msgs::Anchor &msg) {
    anchor_msgs::SizeAttribute size_msg;
    size_msg.data = this->_data;
    size_msg.symbols = this->_symbols;
    msg.size = size_msg;
  }
  void SizeAttribute::populate(anchor_msgs::Display &msg) {
    msg.size = _symbols[0];
  }

  float SizeAttribute::match(const AttributePtr &query_ptr) {

    // Typecast the query attribute pointer
    SizeAttribute *raw_ptr = dynamic_cast<SizeAttribute*>(query_ptr.get());
    assert( raw_ptr != nullptr );

    // Jaccard similarity coefficient
    float min = 0.0, max = 0.0;
    min += raw_ptr->_data.x < _data.x ? raw_ptr->_data.x : _data.x;
    min += raw_ptr->_data.y < _data.y ? raw_ptr->_data.y : _data.y;
    min += raw_ptr->_data.z < _data.z ? raw_ptr->_data.z : _data.z;
    max += raw_ptr->_data.x > _data.x ? raw_ptr->_data.x : _data.x;
    max += raw_ptr->_data.y > _data.y ? raw_ptr->_data.y : _data.y;
    max += raw_ptr->_data.z > _data.z ? raw_ptr->_data.z : _data.z;
    if( max > 0.0 ) {
      return min / max;
    }
    return 0.0;
  }

  bool SizeAttribute::update(const unique_ptr<AttributeCommon> &new_ptr) {

    // Typecast the new atribute pointer
    SizeAttribute* raw_ptr = dynamic_cast<SizeAttribute*>(new_ptr.get());
    assert( raw_ptr != nullptr );

    this->_data = raw_ptr->_data;
  }

  string SizeAttribute::toString() {
    return _symbols[0];
  }

} // namespace anchoring


/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    This program 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 General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <deque>
#include <boost/cstdint.hpp>
#include <math.h>
#include <algorithm>

#include "graphics.hpp"
#include "asserts.hpp"
#include "color_utils.hpp"
#include "entity.hpp"
#include "foreach.hpp"
#include "formula.hpp"
#include "frame.hpp"
#include "particle_system.hpp"
#include "preferences.hpp"
#include "string_utils.hpp"
#include "texture.hpp"
#include "variant_utils.hpp"
#include "weather_particle_system.hpp"
#include "water_particle_system.hpp"
#include "raster.hpp"

namespace {

class particle_animation {
public:
	explicit particle_animation(variant node) :
	  id_(node["id"].as_string()),
	  texture_(graphics::texture::get(node["image"].as_string())),
	  duration_(node["duration"].as_int()),
	  reverse_frame_(node["reverse"].as_bool()),
	  loops_(node["loops"].as_bool(false))
	{
		rect base_area(node.has_key("rect") ? rect(node["rect"]) :
	           rect(node["x"].as_int(),
	                node["y"].as_int(),
	                node["w"].as_int(),
	                node["h"].as_int()));
		width_  = base_area.w()*node["scale"].as_int(2);
		height_ = base_area.h()*node["scale"].as_int(2);
		int nframes = node["frames"].as_int(1);
		if(nframes < 1) {
			nframes = 1;
		}

		const int nframes_per_row = node["frames_per_row"].as_int(-1);
		const int pad = node["pad"].as_int();

		frame frame_obj(node);

		int row = 0, col = 0;
		for(int n = 0; n != nframes; ++n) {
			const frame::frame_info& info = frame_obj.frame_layout()[n];
			const rect& area = info.area;

			frame_area a;
			a.u1 = GLfloat(area.x())/GLfloat(texture_.width());
			a.u2 = GLfloat(area.x2())/GLfloat(texture_.width());
			a.v1 = GLfloat(area.y())/GLfloat(texture_.height());
			a.v2 = GLfloat(area.y2())/GLfloat(texture_.height());

			a.x_adjust = info.x_adjust*2;
			a.y_adjust = info.y_adjust*2;
			a.x2_adjust = info.x2_adjust*2;
			a.y2_adjust = info.y2_adjust*2;

			frames_.push_back(a);

			++col;
			if(col == nframes_per_row) {
				col = 0;
				++row;
			}
		}
	}

	struct frame_area {
		GLfloat u1, v1, u2, v2;
		int x_adjust, y_adjust, x2_adjust, y2_adjust;
	};

	const frame_area& get_frame(int t) const {
		int index = t/duration_;
		if(index < 0) {
			index = 0;
		} else if(index >= frames_.size()) {
			if(loops_ && !reverse_frame_) {
				index = index%frames_.size();
			} else if (loops_ && reverse_frame_){
				index = (running_in_reverse(index)? frames_.size()- 1 - index%frames_.size(): index%frames_.size());
			} else {
				index = frames_.size() - 1;
			}
		}

		return frames_[index];
	}
	bool running_in_reverse(int current_frame) const
	{
		return current_frame % (2* frames_.size()) >= frames_.size();
	}

	void set_texture() const {
		texture_.set_as_current_texture();
	}

	int width() const { return width_; }
	int height() const { return height_; }
private:
	std::string id_;
	graphics::texture texture_;

	std::vector<frame_area> frames_;
	int duration_;
	int reverse_frame_;
	int width_, height_;
	bool loops_;
};

struct simple_particle_system_info {
	simple_particle_system_info(variant node)
	  : spawn_rate_(node["spawn_rate"].as_int(1)),
	    spawn_rate_random_(node["spawn_rate_random"].as_int()),
	    system_time_to_live_(node["system_time_to_live"].as_int(-1)),
	    time_to_live_(node["time_to_live"].as_int(50)),
	    min_x_(node["min_x"].as_int(0)),
	    max_x_(node["max_x"].as_int(0)),
	    min_y_(node["min_y"].as_int(0)),
	    max_y_(node["max_y"].as_int(0)),
		velocity_x_(node["velocity_x"].as_int(0)),
		velocity_y_(node["velocity_y"].as_int(0)),
		velocity_x_rand_(node["velocity_x_random"].as_int(0)),
		velocity_y_rand_(node["velocity_y_random"].as_int(0)),
		velocity_magnitude_(node["velocity_magnitude"].as_int(0)),
		velocity_magnitude_rand_(node["velocity_magnitude_random"].as_int(0)),
		velocity_rotate_(node["velocity_rotate"].as_int(0)),
		velocity_rotate_rand_(node["velocity_rotate_random"].as_int(0)),
		accel_x_(node["accel_x"].as_int(0)),
		accel_y_(node["accel_y"].as_int(0)),
		pre_pump_cycles_(node["pre_pump_cycles"].as_int(0)),
		delta_r_(node["delta_r"].as_int(0)),
		delta_g_(node["delta_g"].as_int(0)),
		delta_b_(node["delta_b"].as_int(0)),
		delta_a_(node["delta_a"].as_int(0)),
		random_schedule_(false)

	{
		if(node.has_key("velocity_x_schedule")) {
			velocity_x_schedule_ = node["velocity_x_schedule"].as_list_int();
		}

		if(node.has_key("velocity_y_schedule")) {
			velocity_y_schedule_ = node["velocity_y_schedule"].as_list_int();
		}

		random_schedule_ = node["random_schedule"].as_bool(velocity_x_schedule_.empty() == false || velocity_y_schedule_.empty() == false);
	}
	int spawn_rate_, spawn_rate_random_;
	int system_time_to_live_;
	int time_to_live_;
	int min_x_, max_x_, min_y_, max_y_;
	int velocity_x_, velocity_y_;
	int velocity_x_rand_, velocity_y_rand_;
	int velocity_magnitude_, velocity_magnitude_rand_;
	int velocity_rotate_, velocity_rotate_rand_;
	int accel_x_, accel_y_;

	int pre_pump_cycles_;  //# of cycles to pre-emptively simulate so the particle system appears to have been running for a while, rather than visibly starting to emit particles just when the player walks onscreen

	int delta_r_, delta_g_, delta_b_, delta_a_;

	std::vector<int> velocity_x_schedule_, velocity_y_schedule_;

	bool random_schedule_;
};

class simple_particle_system_factory : public particle_system_factory {
public:
	explicit simple_particle_system_factory(variant node);
	~simple_particle_system_factory() {}

	particle_system_ptr create(const entity& e) const;

	std::vector<particle_animation> frames_;

	simple_particle_system_info info_;
};

simple_particle_system_factory::simple_particle_system_factory(variant node)
  : info_(node)
{
	foreach(variant frame_node, node["animation"].as_list()) {
		frames_.push_back(particle_animation(frame_node));
	}
}

class simple_particle_system : public particle_system
{
public:
	simple_particle_system(const entity& e, const simple_particle_system_factory& factory);
	~simple_particle_system() {}

	bool is_destroyed() const { return info_.system_time_to_live_ == 0 || info_.spawn_rate_ < 0 && particles_.empty(); }
	bool should_save() const { return info_.spawn_rate_ >= 0; }
	void process(const entity& e);
	void draw(const rect& area, const entity& e) const;

private:
	void prepump(const entity& e);

	variant get_value(const std::string& key) const {
		if(key == "spawn_rate") {
			return variant(info_.spawn_rate_);
		} else if(key == "spawn_rate_random") {
			return variant(info_.spawn_rate_random_);
		} else if(key == "system_time_to_live") {
			return variant(info_.system_time_to_live_);
		} else if(key == "time_to_live") {
			return variant(info_.time_to_live_);
		} else if(key == "min_x") {
			return variant(info_.min_x_);
		} else if(key == "max_x") {
			return variant(info_.max_x_);
		} else if(key == "min_y") {
			return variant(info_.min_y_);
		} else if(key == "max_y") {
			return variant(info_.max_y_);
		} else if(key == "velocity_x") {
			return variant(info_.velocity_x_);
		} else if(key == "velocity_y") {
			return variant(info_.velocity_y_);
		} else if(key == "velocity_x_random") {
			return variant(info_.velocity_x_rand_);
		} else if(key == "velocity_y_random") {
			return variant(info_.velocity_y_rand_);
		} else if(key == "velocity_magnitude") {
			return variant(info_.velocity_magnitude_);
		} else if(key == "velocity_magnitude_random") {
			return variant(info_.velocity_magnitude_rand_);
		} else if(key == "velocity_rotate") {
			return variant(info_.velocity_rotate_);
		} else if(key == "velocity_rotate_random") {
			return variant(info_.velocity_rotate_rand_);
		} else if(key == "velocity_rotate") {
			return variant(info_.velocity_rotate_);
		} else if(key == "velocity_rotate_random") {
			return variant(info_.velocity_rotate_rand_);
		} else if(key == "accel_x") {
			return variant(info_.accel_x_);
		} else if(key == "accel_y") {
			return variant(info_.accel_y_);
		} else if(key == "pre_pump_cycles") {
			return variant(info_.pre_pump_cycles_);
		} else if(key == "delta_r") {
			return variant(info_.delta_r_);
		} else if(key == "delta_g") {
			return variant(info_.delta_g_);
		} else if(key == "delta_b") {
			return variant(info_.delta_b_);
		} else if(key == "delta_a") {
			return variant(info_.delta_a_);
		} else {
			return variant();
		}
	}

	void set_value(const std::string& key, const variant& value) {
		if(key == "spawn_rate") {
			info_.spawn_rate_ = value.as_int();
		} else if(key == "spawn_rate_random") {
			info_.spawn_rate_random_ = value.as_int();
		} else if(key == "system_time_to_live") {
			info_.system_time_to_live_ = value.as_int();
		} else if(key == "time_to_live") {
			info_.time_to_live_ = value.as_int();
		} else if(key == "min_x") {
			info_.min_x_ = value.as_int();
		} else if(key == "max_x") {
			info_.max_x_ = value.as_int();
		} else if(key == "min_y") {
			info_.min_y_ = value.as_int();
		} else if(key == "max_y") {
			info_.max_y_ = value.as_int();
		} else if(key == "velocity_x") {
			info_.velocity_x_ = value.as_int();
		} else if(key == "velocity_y") {
			info_.velocity_y_ = value.as_int();
		} else if(key == "velocity_x_random") {
			info_.velocity_x_rand_ = value.as_int();
		} else if(key == "velocity_y_random") {
			info_.velocity_y_rand_ = value.as_int();
		} else if(key == "velocity_magnitude") {
			info_.velocity_magnitude_ = value.as_int();
		} else if(key == "velocity_magnitude_random") {
			info_.velocity_magnitude_rand_ = value.as_int();
		} else if(key == "velocity_rotate") {
			info_.velocity_rotate_ = value.as_int();
		} else if(key == "velocity_rotate_random") {
			info_.velocity_rotate_rand_ = value.as_int();
		} else if(key == "velocity_rotate") {
			info_.velocity_rotate_ = value.as_int();
		} else if(key == "velocity_rotate_random") {
			info_.velocity_rotate_rand_ = value.as_int();
		} else if(key == "accel_x") {
			info_.accel_x_ = value.as_int();
		} else if(key == "accel_y") {
			info_.accel_y_ = value.as_int();
		} else if(key == "pre_pump_cycles") {
			info_.pre_pump_cycles_ = value.as_int();
		} else if(key == "delta_r") {
			info_.delta_r_ = value.as_int();
		} else if(key == "delta_g") {
			info_.delta_g_ = value.as_int();
		} else if(key == "delta_b") {
			info_.delta_b_ = value.as_int();
		} else if(key == "delta_a") {
			info_.delta_a_ = value.as_int();
		}

	}

	const simple_particle_system_factory& factory_;
	simple_particle_system_info info_;

	int cycle_;

	struct particle {
		GLfloat pos[2];
		GLfloat velocity[2];
		const particle_animation* anim;
		int random;
	};

	struct generation {
		int members;
		int created_at;
	};

	std::deque<particle> particles_;
	std::deque<generation> generations_;

	int spawn_buildup_;
};

simple_particle_system::simple_particle_system(const entity& e, const simple_particle_system_factory& factory)
  : factory_(factory), info_(factory.info_), cycle_(0), spawn_buildup_(0)
{
}

void simple_particle_system::prepump(const entity& e)
{
	//cosmetic thing for very slow-moving particles:
	//it looks weird when you walk into a scene, with, say, a column of smoke that's presumably been rising for quite some time,
	//but it only begins rising the moment you arrive.  To overcome this, we can optionally have particle systems pre-simulate their particles
	//for the short period of time (often as low as 4 seconds) needed to eliminate that implementation artifact
	for( int i = 0; i < info_.pre_pump_cycles_; ++i)
	{
		process(e);
	}

}

void simple_particle_system::process(const entity& e)
{
	--info_.system_time_to_live_;
	++cycle_;

	if(cycle_ == 1) {
		prepump(e);
	}

	while(!generations_.empty() && cycle_ - generations_.front().created_at == info_.time_to_live_) {
		particles_.erase(particles_.begin(), particles_.begin() + generations_.front().members);
		generations_.pop_front();
	}

	std::deque<particle>::iterator p = particles_.begin();
	foreach(generation& gen, generations_) {
		for(int n = 0; n != gen.members; ++n) {
			p->pos[0] += p->velocity[0];
			p->pos[1] += p->velocity[1];
			if(e.face_right()) {
				p->velocity[0] += info_.accel_x_/1000.0;
			} else {
				p->velocity[0] -= info_.accel_x_/1000.0;
			}
			p->velocity[1] += info_.accel_y_/1000.0;
			++p;
		}
	}

	if(info_.velocity_x_schedule_.empty() == false) {
		std::deque<particle>::iterator p = particles_.begin();
		foreach(generation& gen, generations_) {

			for(int n = 0; n != gen.members; ++n) {
				const int ncycle = p->random + cycle_ - gen.created_at - 1;
				p->velocity[0] += info_.velocity_x_schedule_[ncycle%info_.velocity_x_schedule_.size()];
				if(cycle_ - gen.created_at > 1) {
					p->velocity[0] -= info_.velocity_x_schedule_[(ncycle-1)%info_.velocity_x_schedule_.size()];
				}

				++p;
			}
		}
	}

	if(info_.velocity_y_schedule_.empty() == false) {
		std::deque<particle>::iterator p = particles_.begin();
		foreach(generation& gen, generations_) {
			for(int n = 0; n != gen.members; ++n) {
				const int ncycle = p->random + cycle_ - gen.created_at - 1;
				p->velocity[1] += info_.velocity_y_schedule_[ncycle%info_.velocity_y_schedule_.size()];
				if(cycle_ - gen.created_at > 1) {
					p->velocity[1] -= info_.velocity_y_schedule_[(ncycle-1)%info_.velocity_y_schedule_.size()];
				}

				++p;
			}
		}
	}

	int nspawn = info_.spawn_rate_;
	if(info_.spawn_rate_random_ > 0) {
		nspawn += rand()%info_.spawn_rate_random_;
	}

	if(nspawn > 0) {
		nspawn += spawn_buildup_;
	}

	spawn_buildup_ = nspawn%1000;
	nspawn /= 1000;

	if(nspawn == 0) {
		return;
	}

	generation new_gen;
	new_gen.members = nspawn;
	new_gen.created_at = cycle_;

	generations_.push_back(new_gen);

	while(nspawn-- > 0) {
		particle p;
		p.pos[0] = e.face_right() ? (e.x() + info_.min_x_) : (e.x() + e.current_frame().width() - info_.max_x_);
		p.pos[1] = e.y() + info_.min_y_;
		p.velocity[0] = info_.velocity_x_/1000.0;
		p.velocity[1] = info_.velocity_y_/1000.0;

		if(info_.velocity_x_rand_ > 0) {
			p.velocity[0] += (rand()%info_.velocity_x_rand_)/1000.0;
		}

		if(info_.velocity_y_rand_ > 0) {
			p.velocity[1] += (rand()%info_.velocity_y_rand_)/1000.0;
		}

		int velocity_magnitude = info_.velocity_magnitude_;
		if(info_.velocity_magnitude_rand_ > 0) {
			velocity_magnitude += rand()%info_.velocity_magnitude_rand_;
		}

		if(velocity_magnitude) {
			int rotate_velocity = info_.velocity_rotate_;
			if(info_.velocity_rotate_rand_) {
				rotate_velocity += rand()%info_.velocity_rotate_rand_;
			}

			const GLfloat rotate_radians = (GLfloat(rotate_velocity)/360.0)*3.14*2.0;
			const GLfloat magnitude = velocity_magnitude/1000.0;
			p.velocity[0] += sin(rotate_radians)*magnitude;
			p.velocity[1] += cos(rotate_radians)*magnitude;
		}

		ASSERT_GT(factory_.frames_.size(), 0);
		p.anim = &factory_.frames_[rand()%factory_.frames_.size()];

		const int diff_x = info_.max_x_ - info_.min_x_;
		if(diff_x > 0) {
			p.pos[0] += (rand()%(diff_x*1000))/1000.0;
		}

		const int diff_y = info_.max_y_ - info_.min_y_;
		if(diff_y > 0) {
			p.pos[1] += (rand()%(diff_y*1000))/1000.0;
		}

		if(!e.face_right()) {
			p.velocity[0] = -p.velocity[0];
		}

		if(info_.random_schedule_) {
			p.random = rand();
		} else {
			p.random = 0;
		}

		particles_.push_back(p);
	}
}

void simple_particle_system::draw(const rect& area, const entity& e) const
{
	if(particles_.empty()) {
		return;
	}

	std::deque<particle>::const_iterator p = particles_.begin();

	//all particles must have the same texture, so just set it once.
	p->anim->set_texture();
	std::vector<GLfloat>& varray = graphics::global_vertex_array();
	std::vector<GLfloat>& tcarray = graphics::global_texcoords_array();
	std::vector<GLbyte>& carray = graphics::global_vertex_color_array();

	const int facing = e.face_right() ? 1 : -1;

	carray.clear();
	varray.clear();
	tcarray.clear();
	foreach(const generation& gen, generations_) {
		for(int n = 0; n != gen.members; ++n) {
			const particle_animation* anim = p->anim;
			const particle_animation::frame_area& f = anim->get_frame(cycle_ - gen.created_at);

			if(info_.delta_a_){
				//Spare the bandwidth if we're opaque
				const int alpha_level = std::max(256 - info_.delta_a_*(cycle_ - gen.created_at), 0);
				const int red = 255;
				const int green = 255;
				const int blue = 255;
				for( int i = 0; i < 6; ++i){
					carray.push_back(red); carray.push_back(green); carray.push_back(blue); carray.push_back(alpha_level);
				}

			}
			//draw the first point twice, to allow drawing all particles
			//in one drawing operation.

			tcarray.push_back(graphics::texture::get_coord_x(f.u1));
			tcarray.push_back(graphics::texture::get_coord_y(f.v1));
			varray.push_back(p->pos[0] + f.x_adjust*facing);
			varray.push_back(p->pos[1] + f.y_adjust);
			tcarray.push_back(graphics::texture::get_coord_x(f.u1));
			tcarray.push_back(graphics::texture::get_coord_y(f.v1));
			varray.push_back(p->pos[0] + f.x_adjust*facing);
			varray.push_back(p->pos[1] + f.y_adjust);

			tcarray.push_back(graphics::texture::get_coord_x(f.u2));
			tcarray.push_back(graphics::texture::get_coord_y(f.v1));
			varray.push_back(p->pos[0] + (anim->width() - f.x2_adjust)*facing);
			varray.push_back(p->pos[1] + f.y_adjust);
			tcarray.push_back(graphics::texture::get_coord_x(f.u1));
			tcarray.push_back(graphics::texture::get_coord_y(f.v2));
			varray.push_back(p->pos[0] + f.x_adjust*facing);
			varray.push_back(p->pos[1] + anim->height() - f.y2_adjust);

			//draw the last point twice.
			tcarray.push_back(graphics::texture::get_coord_x(f.u2));
			tcarray.push_back(graphics::texture::get_coord_y(f.v2));
			varray.push_back(p->pos[0] + (anim->width() - f.x2_adjust)*facing);
			varray.push_back(p->pos[1] + anim->height() - f.y2_adjust);
			tcarray.push_back(graphics::texture::get_coord_x(f.u2));
			tcarray.push_back(graphics::texture::get_coord_y(f.v2));
			varray.push_back(p->pos[0] + (anim->width() - f.x2_adjust)*facing);
			varray.push_back(p->pos[1] + anim->height() - f.y2_adjust);
			++p;
		}
	}

#if defined(USE_SHADERS)
	if(info_.delta_a_) {
		gles2::manager gles2_manager(gles2::get_texcol_shader());
		gles2::active_shader()->shader()->color_array(4, GL_UNSIGNED_BYTE, GL_TRUE, 0, &carray.front());
		gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, GL_FALSE, 0, &varray.front());
		gles2::active_shader()->shader()->texture_array(2, GL_FLOAT, GL_FALSE, 0, &tcarray.front());
		glDrawArrays(GL_TRIANGLE_STRIP, 0, varray.size()/2);
	} else {
		gles2::active_shader()->prepare_draw();
		gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, GL_FALSE, 0, &varray.front());
		gles2::active_shader()->shader()->texture_array(2, GL_FLOAT, GL_FALSE, 0, &tcarray.front());
		glDrawArrays(GL_TRIANGLE_STRIP, 0, varray.size()/2);
	}
#else
	if(info_.delta_a_){
		glEnableClientState(GL_COLOR_ARRAY);
		glColorPointer(4, GL_UNSIGNED_BYTE, 0, &carray.front());
	}

	glVertexPointer(2, GL_FLOAT, 0, &varray.front());
	glTexCoordPointer(2, GL_FLOAT, 0, &tcarray.front());
	glDrawArrays(GL_TRIANGLE_STRIP, 0, varray.size()/2);

	if(info_.delta_a_){
		glDisableClientState(GL_COLOR_ARRAY);
	}
#endif
	glColor4f(1.0, 1.0, 1.0, 1.0);
}

particle_system_ptr simple_particle_system_factory::create(const entity& e) const
{
	return particle_system_ptr(new simple_particle_system(e, *this));
}

struct point_particle_info
{
	explicit point_particle_info(variant node)
	  : generation_rate_millis(node["generation_rate_millis"].as_int()),
	    pos_x(node["pos_x"].as_int()*1024),
	    pos_y(node["pos_y"].as_int()*1024),
	    pos_x_rand(node["pos_x_rand"].as_int()*1024),
	    pos_y_rand(node["pos_y_rand"].as_int()*1024),
	    velocity_x(node["velocity_x"].as_int()),
	    velocity_y(node["velocity_y"].as_int()),
		accel_x(node["accel_x"].as_int()),
		accel_y(node["accel_y"].as_int()),
	    velocity_x_rand(node["velocity_x_rand"].as_int()),
	    velocity_y_rand(node["velocity_y_rand"].as_int()),
		dot_size(node["dot_size"].as_int(1)*(preferences::double_scale() ? 2 : 1)),
		dot_rounded(node["dot_rounded"].as_bool(false)),
	    time_to_live(node["time_to_live"].as_int()),
	    time_to_live_max(node["time_to_live_rand"].as_int() + time_to_live) {

		if(node.has_key("colors")) {
			const std::vector<variant> colors_vec = node["colors"].as_list();
			foreach(const variant& col, colors_vec) {
				unsigned int val = strtoul(col.as_string().c_str(), NULL, 16);
				unsigned char* c = reinterpret_cast<unsigned char*>(&val);
#if SDL_BYTEORDER == SDL_LIL_ENDIAN
				std::reverse(c, c+4);
#endif
				colors.push_back(val);
			}
		}

		if(node.has_key("colors_expression")) {
			const variant v = game_logic::formula(node["colors_expression"]).execute();
			for(int n = 0; n != v.num_elements(); ++n) {
				const variant u = v[n];
				ASSERT_LOG(u.num_elements() == 4, "UNEXPECTED colors_expression: " << u.to_debug_string());
				const unsigned int r = u[0].as_int();
				const unsigned int g = u[1].as_int();
				const unsigned int b = u[2].as_int();
				const unsigned int a = u[3].as_int();
				unsigned int val = (r << 24) + (g << 16) + (b << 8) + a;

#if SDL_BYTEORDER == SDL_LIL_ENDIAN
				unsigned char* c = reinterpret_cast<unsigned char*>(&val);
				std::reverse(c, c+4);
#endif
				colors.push_back(val);
			}
		}

		std::reverse(colors.begin(), colors.end());

		ttl_divisor = time_to_live_max/(colors.size()-1);

		rgba[0] = node["red"].as_int();
		rgba[1] = node["green"].as_int();
		rgba[2] = node["blue"].as_int();
		rgba[3] = node["alpha"].as_int(255);
		rgba_rand[0] = node["red_rand"].as_int();
		rgba_rand[1] = node["green_rand"].as_int();
		rgba_rand[2] = node["blue_rand"].as_int();
		rgba_rand[3] = node["alpha_rand"].as_int();
		rgba_delta[0] = node["red_delta"].as_int();
		rgba_delta[1] = node["green_delta"].as_int();
		rgba_delta[2] = node["blue_delta"].as_int();
		rgba_delta[3] = node["alpha_delta"].as_int();
	}

	int generation_rate_millis;
	int pos_x, pos_y, pos_x_rand, pos_y_rand;
	int velocity_x, velocity_y, velocity_x_rand, velocity_y_rand;
	int accel_x, accel_y;
	int time_to_live, time_to_live_max;
	unsigned char rgba[4];
	unsigned char rgba_rand[4];
	char rgba_delta[4];
	int dot_size;
	bool dot_rounded;

	std::vector<unsigned int> colors;
	int ttl_divisor;
};

class point_particle_system : public particle_system
{
public:
	point_particle_system(const entity& obj, const point_particle_info& info) : obj_(obj), info_(info), particle_generation_(0), generation_rate_millis_(info.generation_rate_millis), pos_x_(info.pos_x), pos_x_rand_(info.pos_x_rand), pos_y_(info.pos_y), pos_y_rand_(info.pos_y_rand) {
	}

	void process(const entity& e) {
		particle_generation_ += generation_rate_millis_;

		particles_.erase(std::remove_if(particles_.begin(), particles_.end(), particle_destroyed), particles_.end());

		for(std::vector<particle>::iterator p = particles_.begin();
		    p != particles_.end(); ++p) {
			p->pos_x += p->velocity_x;
			p->pos_y += p->velocity_y;
			if(e.face_right()) {
				p->velocity_x += info_.accel_x/1000.0;
			} else {
				p->velocity_x -= info_.accel_x/1000.0;
			}
			p->velocity_y += info_.accel_y/1000.0;
			p->rgba[0] = std::min(std::max(0, p->rgba[0] + info_.rgba_delta[0]), 255);
			p->rgba[1] = std::min(std::max(0, p->rgba[1] + info_.rgba_delta[1]), 255);
			p->rgba[2] = std::min(std::max(0, p->rgba[2] + info_.rgba_delta[2]), 255);
			p->rgba[3] = std::min(std::max(0, p->rgba[3] + info_.rgba_delta[3]), 255);
			p->ttl--;
		}

		while(particle_generation_ >= 1000) {
			//std::cerr << "PARTICLE X ORIGIN: " << pos_x_;
			particles_.push_back(particle());
			particle& p = particles_.back();
			p.ttl = info_.time_to_live;
			if(info_.time_to_live_max != info_.time_to_live) {
				p.ttl += rand()%(info_.time_to_live_max - info_.time_to_live);
			}

			p.velocity_x = info_.velocity_x;
			p.velocity_y = info_.velocity_y;

			if(info_.velocity_x_rand) {
				p.velocity_x += rand()%info_.velocity_x_rand;
			}

			if(info_.velocity_y_rand) {
				p.velocity_y += rand()%info_.velocity_y_rand;
			}

			p.pos_x = e.x()*1024 + pos_x_;
			p.pos_y = e.y()*1024 + pos_y_;

			if(pos_x_rand_) {
				p.pos_x += rand()%pos_x_rand_;
			}

			if(pos_y_rand_) {
				p.pos_y += rand()%pos_y_rand_;
			}

			p.rgba[0] = info_.rgba[0];
			p.rgba[1] = info_.rgba[1];
			p.rgba[2] = info_.rgba[2];
			p.rgba[3] = info_.rgba[3];

			if(info_.rgba_rand[0]) {
				p.rgba[0] = std::min(std::max(0, p.rgba[0] + rand()%info_.rgba_rand[0]), 255);
			}

			if(info_.rgba_rand[1]) {
				p.rgba[1] = std::min(std::max(0, p.rgba[1] + rand()%info_.rgba_rand[1]), 255);
			}

			if(info_.rgba_rand[2]) {
				p.rgba[2] = std::min(std::max(0, p.rgba[2] + rand()%info_.rgba_rand[2]), 255);
			}

			if(info_.rgba_rand[3]) {
				p.rgba[3] = std::min(std::max(0, p.rgba[3] + rand()%info_.rgba_rand[3]), 255);
			}

			particle_generation_ -= 1000;
		}
	}

	void draw(const rect& area, const entity& e) const {
		if(particles_.empty()) {
			return;
		}

		static std::vector<GLshort> vertex;
		static std::vector<unsigned int> colors;
		vertex.resize(particles_.size()*2);
		colors.resize(particles_.size());

		unsigned int* c = &colors[0];
		GLshort* v = &vertex[0];
		for(std::vector<particle>::const_iterator p = particles_.begin();
		    p != particles_.end(); ++p) {
			*v++ = p->pos_x/1024;
			*v++ = p->pos_y/1024;
			if(info_.colors.size() >= 2) {
				*c++ = info_.colors[p->ttl/info_.ttl_divisor];
			} else {
				*c++ = p->color;
			}
		}

		glColor4f(1.0, 1.0, 1.0, 1.0);

#if defined(USE_SHADERS)
		// Not dealing with GL_POINT_SMOOTH right now -- this would probably be better as a frgament shader.
		glPointSize(info_.dot_size);
		gles2::manager gles2_manager(gles2::get_simple_col_shader());
		gles2::active_shader()->shader()->vertex_array(2, GL_SHORT, GL_FALSE, 0, &vertex[0]);
		gles2::active_shader()->shader()->color_array(4, GL_UNSIGNED_BYTE, GL_TRUE, 0, &colors[0]);
		glDrawArrays(GL_POINTS, 0, particles_.size());
#else
		glDisable(GL_TEXTURE_2D);
		glDisableClientState(GL_TEXTURE_COORD_ARRAY);
		glEnableClientState(GL_COLOR_ARRAY);
		if(info_.dot_rounded){
			glEnable( GL_POINT_SMOOTH );
		}
		glPointSize(info_.dot_size);

		glVertexPointer(2, GL_SHORT, 0, &vertex[0]);
		glColorPointer(4, GL_UNSIGNED_BYTE, 0, &colors[0]);
		glDrawArrays(GL_POINTS, 0, particles_.size());

		glDisableClientState(GL_COLOR_ARRAY);
		glEnableClientState(GL_TEXTURE_COORD_ARRAY);
		glEnable(GL_TEXTURE_2D);
		if(info_.dot_rounded){
			glDisable( GL_POINT_SMOOTH );
		}
#endif
		glColor4f(1.0, 1.0, 1.0, 1.0);
	}
private:
	const entity& obj_;
	const point_particle_info& info_;

	struct particle {
		GLshort velocity_x, velocity_y;
		int pos_x, pos_y;
		union { unsigned int color; unsigned char rgba[4]; };
		int ttl;
	};

	static bool particle_destroyed(const particle& p) { return p.ttl <= 0; }

	int particle_generation_;
	int generation_rate_millis_;
	int pos_x_, pos_x_rand_, pos_y_, pos_y_rand_;
	std::vector<particle> particles_;

	variant get_value(const std::string& key) const {
		return variant();
	}

	void set_value(const std::string& key, const variant& value) {
		if(key == "generation_rate" || key == "generation_rate_millis") {
			generation_rate_millis_ = value.as_int();
		} else if (key == "pos_x") {
			pos_x_ = value.as_int()*1024;
		} else if (key == "pos_x_rand") {
			pos_x_rand_ = value.as_int()*1024;
		} else if (key == "pos_y") {
			pos_y_ = value.as_int()*1024;
		} else if (key == "pos_y_rand") {
			pos_y_rand_ = value.as_int()*1024;
		}
	}
};

class point_particle_system_factory : public particle_system_factory
{
public:
	explicit point_particle_system_factory(variant node)
	  : info_(node)
	{}

	particle_system_ptr create(const entity& e) const {
		return particle_system_ptr(new point_particle_system(e, info_));
	}

private:
	point_particle_info info_;
};

}

const_particle_system_factory_ptr particle_system_factory::create_factory(variant node)
{
	const std::string& type = node["type"].as_string();
	if(type == "simple") {
		return const_particle_system_factory_ptr(new simple_particle_system_factory(node));
	} else if (type == "weather") {
		return const_particle_system_factory_ptr(new weather_particle_system_factory(node));
	} else if (type == "water") {
		return const_particle_system_factory_ptr(new water_particle_system_factory(node));
	} else if(type == "point") {
		return const_particle_system_factory_ptr(new point_particle_system_factory(node));
	}

	ASSERT_LOG(false, "Unrecognized particle system type: " << node["type"].as_string());
}

particle_system_factory::~particle_system_factory()
{
}

particle_system::~particle_system()
{
}


/** $Id: histogram.cxx 172050 2019-03-15 12:47:22Z flauber $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 172050 $
 * $Date: 2019-03-15 06:47:22 -0600 (Fri, 15 Mar 2019) $
 */

#include <MuonGun/histogram.h>
#include <boost/foreach.hpp>
#include <boost/python/slice.hpp>

namespace bp = boost::python;

#if PY_MAJOR_VERSION >= 3
static PyObject *
PyBuffer_FromMemory(void *memory, size_t size)
{
	Py_buffer buf;
	memset(&buf, 0, sizeof(buf));
	buf.buf = memory;
	buf.len = size;
	buf.readonly = true;
	buf.ndim = 1;
	buf.itemsize = sizeof(double);
        size_t s= size/sizeof(double);
        assert(s<= INT_MAX);
	Py_ssize_t shape[] = { static_cast<Py_ssize_t>(s) };
	buf.shape = shape;
	return PyMemoryView_FromBuffer(&buf);
}
#endif

static bp::object
to_dashi(boost::shared_ptr<I3MuonGun::histogram::histogram_base> h)
{
	bp::object histmodule = bp::import("dashi.histogram");
	bp::object numpy = bp::import("numpy");
	bp::list edges;
	BOOST_FOREACH(const std::vector<double> &dim, h->binedges())
		edges.append(bp::list(dim));

	bp::object dhist = histmodule.attr("histogram")(h->ndim(), edges);

	bp::list shape;
	size_t ndim = h->ndim(); (void) ndim;
	size_t tsize = 1;
	BOOST_FOREACH(size_t i, h->shape()) {
		tsize *= i;
		shape.append(i);
	}
	bp::tuple shapet(shape);
	bp::object dtype = numpy.attr("float64");

	bp::object buffer = bp::object(bp::handle<>(PyBuffer_FromMemory(
	    (void*)h->raw_bincontent(), sizeof(double)*tsize)));
	dhist.attr("_h_bincontent").attr("__setitem__")(bp::slice(),
	    numpy.attr("ndarray")(shapet, dtype, buffer));

	buffer =  bp::object(bp::handle<>(PyBuffer_FromMemory(
	    (void*)h->raw_squaredweights(), sizeof(double)*tsize)));
	dhist.attr("_h_squaredweights").attr("__setitem__")(bp::slice(),
	    numpy.attr("ndarray")(shapet, dtype, buffer));

	return dhist;
}

static boost::shared_ptr<I3MuonGun::histogram::histogram_base> test_histogram()
{
	using namespace I3MuonGun::histogram;

	histogram<3>::bin_specification edges;
	{
		using namespace binning;
		// edges[0] = uniform<>::create(0, 1, 11);
		edges[0] = uniform<cosine>::create(0, M_PI/2., 11);
		// edges[0] = boost::get<boost::shared_ptr<binning::scheme> >(edges[0])->edges();
		edges[1] = uniform<binning::log10>::create(1e3, 1e5, 11);
		edges[2] = uniform<power<2> >::create(1, 10, 11);
	}

	boost::shared_ptr<histogram<3> > h(new histogram<3>(edges));

	boost::array<double, 3> values = {{0.5, 1e4, 8}};

	for (int i=0; i < 25; i++) {
		values[0] = 1e-1*i;
		h->fill(values);
	}

	return h;
}

void register_histogram()
{
	using namespace I3MuonGun::histogram;
	namespace bp = boost::python;

	bp::def("test_histogram", &test_histogram);

	bp::class_<histogram_base, boost::shared_ptr<histogram_base>, boost::noncopyable>("histogram", bp::no_init)
		.def("to_dashi", &to_dashi)
			;
	try {
		bp::import("dashi.histogram");
		bp::import("numpy");
	} catch (const bp::error_already_set&) {

		PyErr_Clear();
	}

}