text:
/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2014-, Open Perception, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * * Neither the name of the copyright holder(s) nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Filippo Basso [bassofil@dei.unipd.it]
 *         Matteo Munaro [matteo.munaro@dei.unipd.it]
 *
 */

#include <ros/ros.h>
#include <opt_msgs/TrackArray.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>

#include <open_ptrack/tracking/track.h>

#include <dynamic_reconfigure/server.h>
#include <tracking/TrackerSmootherConfig.h>

namespace open_ptrack
{
namespace tracking
{

typedef ::tracking::TrackerSmootherConfig Config;
typedef ::dynamic_reconfigure::Server<Config> ReconfigureServer;

struct LastData
{
  ros::Time last_visible_time;
  opt_msgs::TrackArray last_msg;
};

struct TrackState
{
  Eigen::Array2d position_, velocity_, acceleration_;

  void predict(double delta_t, Eigen::Array2d & position, Eigen::Array2d & velocity, Eigen::Array2d & acceleration)
  {
    position = position_ + delta_t * (velocity_ + delta_t * 0.5 * acceleration_);
    velocity = velocity_ + delta_t * acceleration_;
    acceleration  = acceleration_;
  }

  void predict(double delta_t)
  {
    position_ += delta_t * (velocity_ + delta_t * 0.5 * acceleration_);
    velocity_ += delta_t * acceleration_;
  }

  void update(const Eigen::Array2d & measured_position, double delta_t, double alpha, double beta, double phi)
  {
    Eigen::Array2d residual = measured_position - position_;
    position_ += alpha * residual;
    velocity_ += beta / delta_t * residual;
    acceleration_ += phi * 0.5 / (delta_t * delta_t) * residual;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

class TrackSmoother
{

public:

  TrackSmoother(ros::NodeHandle & node_handle)
    : node_handle_(node_handle),
      reconfigure_server_(new ReconfigureServer(config_mutex_, node_handle_)),
      rate_(ros::Rate(30.0))
  {
    double rate_d, track_lifetime_with_no_detections, heartbeat_time;

    node_handle_.param("rate", rate_d, 30.0);
    node_handle_.param("track_lifetime_with_no_detections", track_lifetime_with_no_detections, 1.0);
    node_handle_.param("publish_empty", publish_empty_, true);
    node_handle_.param("heartbeat_time", heartbeat_time, 5.0);
    node_handle_.param("max_history_size", max_history_size_, 1000);

    double sigma_process, sigma_noise;
    node_handle_.param("sigma_process", sigma_process, 1.0);
    node_handle_.param("sigma_noise", sigma_noise, 0.0);
    computeWeights(sigma_process, sigma_noise);

    double prediction_duration_d;
    node_handle_.param("prediction_duration", prediction_duration_d, 0.1);
    prediction_duration_ = ros::Duration(prediction_duration_d);

    generateColors();
    history_cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB> >();
    history_cloud_index_ = 0;
    history_cloud_->header.frame_id = "world";

    time_alive_ = ros::Duration(track_lifetime_with_no_detections);
    heartbeat_time_duration_ = ros::Duration(heartbeat_time);

    tracking_sub_ = node_handle_.subscribe<opt_msgs::TrackArray>("input", 100, &TrackSmoother::trackingCallback, this);
    tracking_pub_ = node_handle_.advertise<opt_msgs::TrackArray>("output", 1);
    marker_array_pub_ = node_handle_.advertise<visualization_msgs::MarkerArray>("markers_array", 1);
    history_pub_ = node_handle_.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("history", 1);

    rate_ = ros::Rate(rate_d);

    ReconfigureServer::CallbackType callback = boost::bind(&TrackSmoother::configCallback, this, _1, _2);
    reconfigure_server_->setCallback(callback);

  }

  void configCallback(Config & config, uint32_t level)
  {
    //rate_ = ros::Rate(config.rate);

    config.track_lifetime_with_no_detections = std::max(config.track_lifetime_with_no_detections, rate_.expectedCycleTime().toSec());
    time_alive_ = ros::Duration(config.track_lifetime_with_no_detections);

    heartbeat_time_duration_ = ros::Duration(config.heartbeat_time);
    publish_empty_ = config.publish_empty;

    computeWeights(config.sigma_process, config.sigma_noise);
    prediction_duration_ = ros::Duration(config.prediction_duration);

    max_history_size_ = config.max_history_size;

  }

  void trackingCallback(const opt_msgs::TrackArray::ConstPtr & tracking_msg)
  {
    for (size_t i = 0; i < tracking_msg->tracks.size(); ++i)
    {
      opt_msgs::Track msg_track = tracking_msg->tracks[i];
      opt_msgs::TrackArray msg;
      msg.header = tracking_msg->header;

      std::map<int, LastData>::iterator it = last_data_map_.find(msg_track.id);

      if (it != last_data_map_.end()) // Track exists
      {
        LastData & last_data = it->second;
        double delta_t = (msg.header.stamp - last_data.last_msg.header.stamp).toSec();
        if (delta_t <= 0)
          break;

        if (state_map_.find(msg_track.id) != state_map_.end())
        {
          TrackState & state = *state_map_[msg_track.id];
          state.predict(delta_t);
          config_mutex_.lock();
          state.update(Eigen::Array2d(msg_track.x, msg_track.y), delta_t, alpha_, beta_, gamma_);
          config_mutex_.unlock();
        }
        else
        {
          boost::shared_ptr<TrackState> state = boost::make_shared<TrackState>();
          state->position_ = Eigen::Array2d(msg_track.x, msg_track.y);
          state->velocity_ = Eigen::Array2d(0, 0);//(state->position_ - Eigen::Array2d(last_data.last_msg.tracks[0].x, last_data.last_msg.tracks[0].y)) / delta_t;
          state->acceleration_ = Eigen::Array2d(0, 0);
          state_map_[msg_track.id] = state;
        }

        TrackState & state = *state_map_[msg_track.id];
        msg_track.x = state.position_[0];
        msg_track.y = state.position_[1];
        msg.tracks.push_back(msg_track);

        last_data.last_msg = msg;
        if (msg_track.visibility != opt_msgs::Track::NOT_VISIBLE)
          last_data.last_visible_time = msg.header.stamp;

      }
      else// if (msg_track.visibility != opt_msgs::Track::NOT_VISIBLE)
      {
        LastData data;
        msg.tracks.push_back(msg_track);
        data.last_msg = msg;
        data.last_visible_time = msg.header.stamp;
        last_data_map_[msg_track.id] = data;
      }
    }
  }

  void spin()
  {
    ros::Time last_heartbeat_time = ros::Time::now();

    while (ros::ok())
    {
      ros::spinOnce();

      opt_msgs::TrackArray track_msg;
      visualization_msgs::MarkerArray marker_msg;

      int n = createMsg(track_msg, marker_msg);
      ros::Time current_time = ros::Time::now();

      config_mutex_.lock();
      if (publish_empty_ or n > 0)
      {
        tracking_pub_.publish(track_msg);
        marker_array_pub_.publish(marker_msg);
        history_pub_.publish(history_cloud_);
        last_heartbeat_time = current_time;
      }
      else if (not publish_empty_)
      {
        // Publish a heartbeat message every 'heartbeat_time' seconds
        if ((current_time - last_heartbeat_time) > heartbeat_time_duration_)
        {
          opt_msgs::TrackArray heartbeat_msg;
          heartbeat_msg.header.stamp = current_time;
          heartbeat_msg.header.frame_id = "heartbeat";
          tracking_pub_.publish(heartbeat_msg);
          marker_array_pub_.publish(marker_msg);
          history_pub_.publish(history_cloud_);
          last_heartbeat_time = current_time;
        }
      }
      config_mutex_.unlock();

      rate_.sleep();
    }
  }

private:

  void computeWeights(double sigma_process, double sigma_noise)
  {
    if (sigma_noise < 1e-5)
    {
      alpha_ = 1.0;
      beta_ = gamma_ = 0.0;
    }
    else
    {
      double t = rate_.expectedCycleTime().toSec();
      double lambda = sigma_process * t * t / sigma_noise;
      double b = lambda / 2.0 - 3.0;
      double c = lambda / 2.0 + 3.0;
      double p = c - b * b / 3.0;
      double q = 2.0 * b * b * b / 27.0 - b * c / 3.0 - 1.0;
      double v = std::sqrt(q * q + 4 * p * p * p / 27.0);
      double z = -std::pow(q + v / 2.0, 1.0 / 3.0);
      double s = z - p / (3.0 * z) - b / 3.0;
      alpha_ = 1.0 - s * s;
      beta_ = 2.0 * (1.0 - s) * (1.0 - s);
      gamma_ = beta_ * beta_ / (2 * alpha_);
    }
  }

  void generateColors()
  {
    for (size_t i = 0; i <= 4; ++i)
      for (size_t j = 0; j <= 4; ++j)
        for (size_t k = 0; k <= 4; ++k)
          color_set_.push_back(Eigen::Vector3f(i * 0.25f, j * 0.25f, k * 0.25f));

    std::random_shuffle(color_set_.begin(), color_set_.end());
  }

  void appendToHistory(const LastData & data)
  {
//    if (data.last_msg.tracks[0].visibility == opt_msgs::Track::NOT_VISIBLE)
//      return;

    config_mutex_.lock();
    if (history_cloud_->size() < max_history_size_)
    {
      pcl::PointXYZRGB point;
      history_cloud_->push_back(point);
    }
    config_mutex_.unlock();

    toPointXYZRGB(history_cloud_->points[history_cloud_index_], data);
    history_cloud_index_ = (history_cloud_index_ + 1) % max_history_size_;
  }

  int createMsg(opt_msgs::TrackArray & track_msg,
                visualization_msgs::MarkerArray & marker_msg)
  {
    int added = 0;
    ros::Time now = ros::Time::now();

    track_msg.header.stamp = now;
    history_cloud_->header.stamp = now.toNSec() / 1000;
    std::vector<int> to_remove;

    for (std::map<int, LastData>::iterator it = last_data_map_.begin(); it != last_data_map_.end(); ++it)
    {
      opt_msgs::TrackArray & saved_msg = it->second.last_msg;
      const ros::Time & time = it->second.last_visible_time;

      config_mutex_.lock();
      bool ok = (now - time) < time_alive_;
      bool prediction = (now - time) < prediction_duration_;
      config_mutex_.unlock();

      if (ok)
      {
        if (state_map_.find(it->first) != state_map_.end() and prediction)
        {
          Eigen::Array2d position, velocity, acceleration;
          state_map_[it->first]->predict((now - time).toSec(), position, velocity, acceleration);
          saved_msg.tracks[0].x = position[0];
          saved_msg.tracks[0].y = position[1];
          track_msg.tracks.push_back(saved_msg.tracks[0]);
          track_msg.header.frame_id = "world";
          createMarker(marker_msg, it->second);
          appendToHistory(it->second);
          ++added;
        }
      }
      else
      {
        to_remove.push_back(saved_msg.tracks[0].id);
      }
    }

    for (size_t i = 0; i < to_remove.size(); ++i)
    {
      last_data_map_.erase(to_remove[i]);
      state_map_.erase(to_remove[i]);
    }

    return added;
  }

  void createMarker(visualization_msgs::MarkerArray & msg,
                    const LastData & data)
  {
    const opt_msgs::Track & track = data.last_msg.tracks[0];

//    if(track.visibility == Track::NOT_VISIBLE)
//      return;

    visualization_msgs::Marker marker;

    marker.header.frame_id = data.last_msg.header.frame_id;
    marker.header.stamp = data.last_msg.header.stamp;

    marker.ns = "people";
    marker.id = track.id;

    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = track.x;
    marker.pose.position.y = track.y;
    marker.pose.position.z = track.height / 2;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;

    marker.color.r = color_set_[track.id % color_set_.size()](2);
    marker.color.g = color_set_[track.id % color_set_.size()](1);
    marker.color.b = color_set_[track.id % color_set_.size()](0);
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration(0.2);

    msg.markers.push_back(marker);

    //------------------------------------

    visualization_msgs::Marker text_marker;

    text_marker.header.frame_id = data.last_msg.header.frame_id;
    text_marker.header.stamp = data.last_msg.header.stamp;

    text_marker.ns = "numbers";
    text_marker.id = track.id;

    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.action = visualization_msgs::Marker::ADD;

    std::stringstream ss;
    ss << track.id;
    text_marker.text = ss.str();

    text_marker.pose.position.x = track.x;
    text_marker.pose.position.y = track.y;
    text_marker.pose.position.z = track.height + 0.1;
    text_marker.pose.orientation.x = 0.0;
    text_marker.pose.orientation.y = 0.0;
    text_marker.pose.orientation.z = 0.0;
    text_marker.pose.orientation.w = 1.0;

    text_marker.scale.x = 0.2;
    text_marker.scale.y = 0.2;
    text_marker.scale.z = 0.2;

    text_marker.color.r = color_set_[track.id % color_set_.size()](2);
    text_marker.color.g = color_set_[track.id % color_set_.size()](1);
    text_marker.color.b = color_set_[track.id % color_set_.size()](0);
    text_marker.color.a = 1.0;

    text_marker.lifetime = ros::Duration(0.2);

    msg.markers.push_back(text_marker);
  }

  void toPointXYZRGB(pcl::PointXYZRGB & p,
                     const LastData & data)
  {
    const opt_msgs::Track & track = data.last_msg.tracks[0];

//    if(track.visibility == Track::NOT_VISIBLE)
//      return;

    p.x = float(track.x);
    p.y = float(track.y);
    p.z = float(track.height / 2);
    uchar * rgb_ptr = reinterpret_cast<uchar *>(&p.rgb);
    *rgb_ptr++ = uchar(color_set_[track.id % color_set_.size()](0) * 255.0f);
    *rgb_ptr++ = uchar(color_set_[track.id % color_set_.size()](1) * 255.0f);
    *rgb_ptr++ = uchar(color_set_[track.id % color_set_.size()](2) * 255.0f);
  }

  ros::NodeHandle node_handle_;

  boost::recursive_mutex config_mutex_;
  boost::shared_ptr<ReconfigureServer> reconfigure_server_;

  ros::Subscriber tracking_sub_;
  ros::Publisher tracking_pub_;
  ros::Publisher marker_array_pub_;
  ros::Publisher history_pub_;

  ros::Duration heartbeat_time_duration_;

  bool publish_empty_;

  std::map<int, LastData> last_data_map_;
  ros::Duration time_alive_;
  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > color_set_;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr history_cloud_;
  int max_history_size_;
  size_t history_cloud_index_;

  ros::Rate rate_;
  double alpha_, beta_, gamma_;
  ros::Duration prediction_duration_;

  std::map<int, boost::shared_ptr<TrackState> > state_map_;

};

} // namespace tracking
} // namespace open_ptrack

using open_ptrack::tracking::TrackSmoother;

int main(int argc, char **argv)
{
  // Initialization:
  ros::init(argc, argv, "tracking_filter");
  ros::NodeHandle nh("~");

  TrackSmoother smoother(nh);
  smoother.spin();

  return 0;
}


text:
// Copyright (c) 2020 The Bitcoin SV developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/test/unit_test.hpp>
#include <netmessagemaker.h>
#include <protocol.h>

#include <vector>

BOOST_AUTO_TEST_SUITE(NetMessageMaker)

BOOST_AUTO_TEST_CASE(Make)
{
    CNetMsgMaker msgMaker {0};
    std::vector<CInv> vToFetch {};

    // Unspecified payload type
    CSerializedNetMsg msg1 { msgMaker.Make(NetMsgType::GETDATA, vToFetch) };
    BOOST_CHECK(msg1.Command() == NetMsgType::GETDATA);
    BOOST_CHECK(msg1.GetPayloadType() == CSerializedNetMsg::PayloadType::UNKNOWN);

    // Specified payload type
    CSerializedNetMsg msg2 { msgMaker.Make(CSerializedNetMsg::PayloadType::BLOCK, NetMsgType::GETDATA, vToFetch) };
    BOOST_CHECK(msg2.Command() == NetMsgType::GETDATA);
    BOOST_CHECK(msg2.GetPayloadType() == CSerializedNetMsg::PayloadType::BLOCK);
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: AustEcon/bitcoin-sv

max_issues_repo_name: wjxee/bitcoin-sv

max_forks_repo_name: wjxee/bitcoin-sv


text:
//          Copyright Oliver Kowalke 2013.
// 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_FIBERS_H
#define BOOST_FIBERS_H

#include <boost/fiber/algo/algorithm.hpp>
#include <boost/fiber/algo/round_robin.hpp>
#include <boost/fiber/algo/shared_work.hpp>
#include <boost/fiber/algo/work_stealing.hpp>
#include <boost/fiber/algo/numa/work_stealing.hpp>
#include <boost/fiber/barrier.hpp>
#include <boost/fiber/buffered_channel.hpp>
#include <boost/fiber/channel_op_status.hpp>
#include <boost/fiber/condition_variable.hpp>
#include <boost/fiber/context.hpp>
#include <boost/fiber/exceptions.hpp>
#include <boost/fiber/fiber.hpp>
#include <boost/fiber/fixedsize_stack.hpp>
#include <boost/fiber/fss.hpp>
#include <boost/fiber/future.hpp>
#include <boost/fiber/numa/pin_thread.hpp>
#include <boost/fiber/numa/topology.hpp>
#include <boost/fiber/mutex.hpp>
#include <boost/fiber/operations.hpp>
#include <boost/fiber/policy.hpp>
#include <boost/fiber/pooled_fixedsize_stack.hpp>
#include <boost/fiber/properties.hpp>
#include <boost/fiber/protected_fixedsize_stack.hpp>
#include <boost/fiber/recursive_mutex.hpp>
#include <boost/fiber/recursive_timed_mutex.hpp>
#include <boost/fiber/scheduler.hpp>
#include <boost/fiber/segmented_stack.hpp>
#include <boost/fiber/timed_mutex.hpp>
#include <boost/fiber/type.hpp>
#include <boost/fiber/unbuffered_channel.hpp>

#endif // BOOST_FIBERS_H

ext: hpp


text:
/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_NV_HPP
#define ODE_NV_HPP

// ODE (not verified)

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/psa.hpp>
#include <kv/ode-param.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

namespace kv{

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
ode_nv(F f, ub::vector<T>& init, const T& start, T& end, ode_param<T> p = ode_param<T>()) {
	int n = init.size();
	int i, j;

	ub::vector< psa<T> > x, y;
	psa<T> torg;
	psa<T> t;

	T deltat;
	ub::vector<T> result;

	T m;

	T radius, radius_tmp;
	T tolerance;
	int n_rad;

	bool save_mode, save_uh, save_rh;

	m = 1.;
	for (i=0; i<n; i++) {
		using std::abs;
		m = std::max(m, abs(init(i)));
	}
	tolerance = m * p.epsilon;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa<T>::mode();
	save_uh = psa<T>::use_history();
	save_rh = psa<T>::record_history();
	psa<T>::mode() = 1;
	psa<T>::use_history() = false;
	psa<T>::record_history() = false;
	#if ODE_FAST == 1
	psa<T>::record_history() = true;
	psa<T>::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa<T>::use_history() = true;
		if (j == p.order - 1) psa<T>::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				using std::abs;
				m = std::max(m, abs(x(i).v(j)));
			}
			if (m == 0.) continue;
			radius_tmp = std::pow((double)m, 1./j);
			if (radius_tmp > radius) radius = radius_tmp;
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
	}

	deltat = end - start;

	if (p.autostep && radius < deltat) {
		end = start + radius;
		deltat = end - start;
	}

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), deltat);
	}

	init = result;

	psa<T>::mode() = save_mode;
	psa<T>::use_history() = save_uh;
	psa<T>::record_history() = save_rh;
}

template <class T, class F>
void
odelong_nv(F f, ub::vector<T>& init, const T& start, const T& end, ode_param<T> p = ode_param<T>()) {

	ub::vector<T> x;
	T t, t1;

	x = init;
	t = start;
	p.set_autostep(true);
	while (1) {
		t1 = end;
		if (t == t1) break;

		ode_nv(f, x, t, t1, p);
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x << "\n";
		}
		t = t1;
	}

	init = x;
}

} // namespace kv

#endif // ODE_NV_HPP

max_stars_repo_name: soonho-tri/kv

max_issues_repo_name: takafumihoriuchi/HyLaGI

max_forks_repo_name: takafumihoriuchi/HyLaGI


text:
/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library 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 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#if defined(MAPNIK_MEMORY_MAPPED_FILE)

// mapnik
#include <mapnik/debug.hpp>
#include <mapnik/util/fs.hpp>
#include <mapnik/mapped_memory_cache.hpp>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include <boost/assert.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/interprocess/file_mapping.hpp>
#pragma GCC diagnostic pop

namespace mapnik
{

template class singleton<mapped_memory_cache, CreateStatic>;

void mapped_memory_cache::clear()
{
#ifdef MAPNIK_THREADSAFE
    std::lock_guard<std::mutex> lock(mutex_);
#endif
    return cache_.clear();
}

bool mapped_memory_cache::insert(std::string const& uri, mapped_region_ptr mem)
{
#ifdef MAPNIK_THREADSAFE
    std::lock_guard<std::mutex> lock(mutex_);
#endif
    return cache_.emplace(uri,mem).second;
}

boost::optional<mapped_region_ptr> mapped_memory_cache::find(std::string const& uri, bool update_cache)
{
#ifdef MAPNIK_THREADSAFE
    std::lock_guard<std::mutex> lock(mutex_);
#endif

    using iterator_type = std::unordered_map<std::string, mapped_region_ptr>::const_iterator;
    boost::optional<mapped_region_ptr> result;
    iterator_type itr = cache_.find(uri);
    if (itr != cache_.end())
    {
        result.reset(itr->second);
        return result;
    }

    if (mapnik::util::exists(uri))
    {
        try
        {
            boost::interprocess::file_mapping mapping(uri.c_str(),boost::interprocess::read_only);
            mapped_region_ptr region(std::make_shared<boost::interprocess::mapped_region>(mapping,boost::interprocess::read_only));
            result.reset(region);
            if (update_cache)
            {
                cache_.emplace(uri, *result);
            }
            return result;
        }
        catch (std::exception const& ex)
        {
            MAPNIK_LOG_ERROR(mapped_memory_cache)
                << "Error loading mapped memory file: '"
                << uri << "' (" << ex.what() << ")";
        }
    }
    /*
    else
    {
        MAPNIK_LOG_WARN(mapped_memory_cache) << "Memory region does not exist file: " << uri;
    }
    */
    return result;
}

}

#endif


text:
// Copyright Louis Dionne 2013-2017
// 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/hana/basic_tuple.hpp>
#include <boost/hana/core/make.hpp>

#include <type_traits>
namespace hana = boost::hana;

constexpr hana::basic_tuple<int, char, double> xs{1, '2', 3.3};
constexpr auto ys = hana::make<hana::basic_tuple_tag>(1, '2', 3.3);
constexpr auto zs = hana::make_basic_tuple(1, '2', 3.3);

static_assert(std::is_same<decltype(ys), decltype(xs)>::value, "");
static_assert(std::is_same<decltype(zs), decltype(xs)>::value, "");

int main() { }


text:
/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/gdeque.h"
#include "galois/Bag.h" // XXX
#include "galois/runtime/TaskWork.h"
#include "galois/worklists/WorkListAlt.h"

#include "llvm/Support/CommandLine.h"
#include "Lonestar/BoilerPlate.h"

#include <boost/math/constants/constants.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include <limits>
#include <iostream>
#include <array>
#include <deque>
#include <list>
#include <strings.h>

const char* name = "Barnshut N-Body Simulator";
const char* desc =
    "Simulation of the gravitational forces in a galactic cluster using the "
    "Barnes-Hut n-body algorithm\n";
const char* url = "barneshut";

enum Algo {
  algoBase,
  algoBlocked,
  algoBlockedC,
  algoOrig,
  algoIndirect,
  algoUnroll,
  algoTree,
  algoTaskOrig,
  algoTaskBlocked
};

namespace cll = llvm::cl;

static cll::opt<int> nbodies("n", cll::desc("Number of bodies"),
                             cll::init(10000));
static cll::opt<int> ntimesteps("steps", cll::desc("Number of steps"),
                                cll::init(1));
static cll::opt<int> seed("seed", cll::desc("Random seed"), cll::init(7));
static cll::opt<Algo> algo(
    cll::desc("Algorithm:"),
    cll::values(clEnumVal(algoBase, "AlgoBase"),
                clEnumVal(algoOrig, "AlgoOrig"),
                clEnumVal(algoIndirect, "AlgoIndirect"),
                clEnumVal(algoUnroll, "AlgoUnroll"),
                clEnumVal(algoTree, "AlgoTree"),
                clEnumVal(algoBlocked, "AlgoBlocked"),
                clEnumVal(algoBlockedC, "AlgoBlockedContinuation"),
                clEnumVal(algoTaskOrig, "AlgoTaskOrig"),
                clEnumVal(algoTaskBlocked, "AlgoTaskBlocked"), clEnumValEnd),
    cll::init(algoOrig));
static cll::opt<int> blockSizeX("blockSizeX",
                                cll::desc("Block size for Blocked algos"),
                                cll::init(8));
static cll::opt<int> blockSizeY("blockSizeY",
                                cll::desc("Block size for Blocked algos"),
                                cll::init(8));
static cll::opt<bool> useTaskRefine("useTaskRefine",
                                    cll::desc("Refine task schedule"),
                                    cll::init(false));
static cll::opt<bool> blockFIFO("blockFIFO",
                                cll::desc("Use FIFO for Blocked algos"),
                                cll::init(false));
static cll::opt<bool> allocateFIFO("allocateFIFO",
                                   cll::desc("Use FIFO allocation of tree"),
                                   cll::init(false));

// XXX(ddn): Change to tbb
#ifdef GALOIS_USE_CILK
#include "tbb/task_scheduler_observer.h"
#include "tbb/task_scheduler_init.h"
static cll::opt<bool>
    useCilkTreeBuild("useTBBTreeBuild",
                     cll::desc("Use Cilk scheduler for tree build"),
                     cll::init(false));
struct FixThreads : public tbb::task_scheduler_observer {
  FixThreads() { observe(true); }
  virtual void on_scheduler_entry(bool is_worker) {
    galois::runtime::LL::initTID();
    unsigned id = galois::runtime::LL::getTID();
    galois::runtime::initPTS();
    galois::runtime::LL::bindThreadToProcessor(id);
  }
};
#else
static bool useCilkTreeBuild;
struct FixThreads {};
#define cilk_spawn
#define cilk_sync
#endif

static const int MaxBlockSizeX = 32;

struct Point {
  double x, y, z;
  Point() : x(0.0), y(0.0), z(0.0) {}
  Point(double _x, double _y, double _z) : x(_x), y(_y), z(_z) {}
  explicit Point(double v) : x(v), y(v), z(v) {}

  double operator[](const int index) const {
    switch (index) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    }
    assert(false && "index out of bounds");
    abort();
  }

  double& operator[](const int index) {
    switch (index) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    }
    assert(false && "index out of bounds");
    abort();
  }

  bool operator==(const Point& other) {
    if (x == other.x && y == other.y && z == other.z)
      return true;
    return false;
  }

  bool operator!=(const Point& other) { return !operator==(other); }

  Point& operator+=(const Point& other) {
    x += other.x;
    y += other.y;
    z += other.z;
    return *this;
  }

  Point& operator*=(double value) {
    x *= value;
    y *= value;
    z *= value;
    return *this;
  }

  double dist2() { return x * x + y * y + z * z; }
};

std::ostream& operator<<(std::ostream& os, const Point& p) {
  os << "(" << p[0] << "," << p[1] << "," << p[2] << ")";
  return os;
}

/**
 * A node in an octree is either an internal node or a body (leaf).
 */
struct Octree {
  Point pos;
  double mass;
  Octree* parent;

  Octree(const Point& p, double m, Octree* parent)
      : pos(p), mass(m), parent(parent) {}
  Octree() {}

  virtual ~Octree() {}
  virtual bool isLeaf() const = 0;
};

struct OctreeInternal : public Octree {
  // Extra space for end of array element needed by
  // ComputeForceBlockedContinuation
  Octree* child[8 + 1];

  OctreeInternal(const Point& p, Octree* parent) : Octree(p, 0.0, parent) {
    bzero(child, sizeof(*child) * (8 + 1));
  }

  //  virtual ~OctreeInternal() {
  //    for (int i = 0; i < 8; i++) {
  //      if (child[i] && !child[i]->isLeaf()) {
  //        delete child[i]; // XXX not needed with heap
  //      }
  //    }
  //  }

  virtual bool isLeaf() const { return false; }
};

struct Body : public Octree, public galois::runtime::Lockable {
  Point vel;
  Point acc;
  Point oldacc;

  Body() {}

  virtual bool isLeaf() const { return true; }
};

std::ostream& operator<<(std::ostream& os, const Body& b) {
  os << "(pos:" << b.pos << " vel:" << b.vel << " acc:" << b.acc
     << " mass:" << b.mass << ")";
  return os;
}

struct BoundingBox {
  Point min;
  Point max;
  explicit BoundingBox(const Point& p) : min(p), max(p) {}
  BoundingBox()
      : min(std::numeric_limits<double>::max()),
        max(std::numeric_limits<double>::min()) {}

  void merge(const BoundingBox& other) {
    for (int i = 0; i < 3; i++) {
      if (other.min[i] < min[i])
        min[i] = other.min[i];
    }
    for (int i = 0; i < 3; i++) {
      if (other.max[i] > max[i])
        max[i] = other.max[i];
    }
  }

  void merge(const Point& other) {
    for (int i = 0; i < 3; i++) {
      if (other[i] < min[i])
        min[i] = other[i];
    }
    for (int i = 0; i < 3; i++) {
      if (other[i] > max[i])
        max[i] = other[i];
    }
  }

  double diameter() const {
    double diameter = max.x - min.x;
    for (int i = 1; i < 3; i++) {
      double t = max[i] - min[i];
      if (diameter < t)
        diameter = t;
    }
    return diameter;
  }

  double radius() const { return diameter() / 2; }

  Point center() const {
    return Point((max.x + min.x) * 0.5, (max.y + min.y) * 0.5,
                 (max.z + min.z) * 0.5);
  }
};

std::ostream& operator<<(std::ostream& os, const BoundingBox& b) {
  os << "(min:" << b.min << " max:" << b.max << ")";
  return os;
}

struct Config {
  const double dtime; // length of one time step
  const double eps;   // potential softening parameter
  const double tol;   // tolerance for stopping recursion, <0.57 to bound error
  const double dthf, epssq, itolsq;

  Config()
      : dtime(0.5), eps(0.05), tol(0.025), dthf(dtime * 0.5), epssq(eps * eps),
        itolsq(1.0 / (tol * tol)) {}
};

Config config;

inline int getIndex(const Point& a, const Point& b) {
  int index = 0;
  if (a.x < b.x)
    index += 1;
  if (a.y < b.y)
    index += 2;
  if (a.z < b.z)
    index += 4;
  return index;
}

inline void updateCenter(Point& p, int index, double radius) {
  for (int i = 0; i < 3; i++) {
    double v = (index & (1 << i)) > 0 ? radius : -radius;
    p[i] += v;
  }
}

// XXX numa vector
typedef galois::gdeque<Body> Bodies;
typedef galois::gdeque<Body*> BodyPtrs;

struct BuildOctreeCilk {
  typedef galois::FixedSizeAllocator<BodyPtrs> BodyPtrsAlloc;
  typedef galois::FixedSizeAllocator<OctreeInternal> OctreeInternalAlloc;

  BodyPtrsAlloc& bodyPtrsAlloc;
  OctreeInternalAlloc& octreeInternalAlloc;

  BuildOctreeCilk(BodyPtrsAlloc& a1, OctreeInternalAlloc& a2)
      : bodyPtrsAlloc(a1), octreeInternalAlloc(a2) {}

  void computeCenterOfMass(OctreeInternal* node) {
    Point accum;
    double mass = 0.0;

    for (int i = 0; i < 8; i++) {
      Octree* child = node->child[i];

      if (child == NULL)
        break;

      double m;
      const Point* p;
      if (child->isLeaf()) {
        Body* n = static_cast<Body*>(child);
        m       = n->mass;
        p       = &n->pos;
      } else {
        OctreeInternal* n = static_cast<OctreeInternal*>(child);
        m                 = n->mass;
        p                 = &n->pos;
      }

      mass += m;
      for (int j = 0; j < 3; j++)
        accum[j] += (*p)[j] * m;
    }

    node->mass = mass;

    if (mass > 0.0) {
      double inv_mass = 1.0 / mass;
      for (int j = 0; j < 3; j++)
        node->pos[j] = accum[j] * inv_mass;
    }
  }

  void buildOctree(BodyPtrs::iterator bb, BodyPtrs::iterator eb,
                   OctreeInternal* node, double radius) {

    typedef std::array<int, 8> Counts;
    typedef std::array<BodyPtrs*, 8> Bags;

    Bags bags;
    Counts counts;

    for (int i = 0; i < 8; ++i) {
      bags[i] = bodyPtrsAlloc.allocate(1);
      bodyPtrsAlloc.construct(bags[i]);
      // bags[i] = new BodyPtrs(); // XXX
      counts[i] = 0;
    }

    for (; bb != eb; ++bb) {
      int i = getIndex(node->pos, (*bb)->pos);
      bags[i]->push_back(*bb);
      ++counts[i];
    }

    radius *= 0.5;

    for (int i = 0; i < 8; ++i) {
      if (counts[i] == 0)
        continue;
      if (counts[i] == 1) {
        node->child[i]         = *bags[i]->begin();
        node->child[i]->parent = node;
        continue;
      }

      Point new_pos(node->pos);
      updateCenter(new_pos, i, radius);
      OctreeInternal* new_node = octreeInternalAlloc.allocate(1);
      octreeInternalAlloc.construct(new_node, new_pos, node);
      // OctreeInternal* new_node = new OctreeInternal(new_pos, node); // XXX
      node->child[i] = new_node;

      cilk_spawn buildOctree(bags[i]->begin(), bags[i]->end(), new_node,
                             radius);
    }
    cilk_sync;

    // Reorganize leaves to be denser up front
    int index = 0;
    for (int i = 0; i < 8; i++) {
      Octree* child = node->child[i];
      if (child == NULL)
        continue;

      if (index != i) {
        node->child[index] = child;
        node->child[i]     = NULL;
      }
      index++;
    }

    computeCenterOfMass(node);
  }
};

struct ComputeCenterOfMass {
  OctreeInternal* node;

  ComputeCenterOfMass(OctreeInternal* n) : node(n) {}

  // void inspect() { } // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>&) {
    Point accum;
    double mass = 0.0;

    for (int i = 0; i < 8; i++) {
      Octree* child = node->child[i];

      if (child == NULL)
        break;

      double m;
      const Point* p;
      if (child->isLeaf()) {
        Body* n = static_cast<Body*>(child);
        m       = n->mass;
        p       = &n->pos;
      } else {
        OctreeInternal* n = static_cast<OctreeInternal*>(child);
        m                 = n->mass;
        p                 = &n->pos;
      }

      mass += m;
      for (int j = 0; j < 3; j++)
        accum[j] += (*p)[j] * m;
    }

    node->mass = mass;

    if (mass > 0.0) {
      double inv_mass = 1.0 / mass;
      for (int j = 0; j < 3; j++)
        node->pos[j] = accum[j] * inv_mass;
    }
  }
};

struct BuildOctree {
  typedef galois::FixedSizeAllocator<BodyPtrs> BodyPtrsAlloc;
  typedef galois::FixedSizeAllocator<OctreeInternal> OctreeInternalAlloc;

  BodyPtrsAlloc& bodyPtrsAlloc;
  OctreeInternalAlloc& octreeInternalAlloc;
  BodyPtrs::iterator bb;
  BodyPtrs::iterator eb;
  OctreeInternal* node;
  double radius;
  galois::Task parent;

  BuildOctree(BodyPtrsAlloc& a1, OctreeInternalAlloc& a2,
              BodyPtrs::iterator _bb, BodyPtrs::iterator _eb, OctreeInternal* n,
              double r, galois::Task p)
      : bodyPtrsAlloc(a1), octreeInternalAlloc(a2), bb(_bb), eb(_eb), node(n),
        radius(r), parent(p) {}

  //  void inspect() { } // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    typedef std::array<int, 8> Counts;
    typedef std::array<BodyPtrs*, 8> Bags;

    Bags bags;
    Counts counts;

    for (int i = 0; i < 8; ++i) {
      bags[i] = bodyPtrsAlloc.allocate(1);
      bodyPtrsAlloc.construct(bags[i]);
      counts[i] = 0;
    }

    for (; bb != eb; ++bb) {
      int i = getIndex(node->pos, (*bb)->pos);
      bags[i]->push_back(*bb);
      ++counts[i];
    }

    radius *= 0.5;

    galois::Task after = ctx.addTask2(ComputeCenterOfMass(node));
    if (parent)
      ctx.addDependence(after, parent);

    for (int i = 0; i < 8; ++i) {
      if (counts[i] == 0)
        continue;
      if (counts[i] == 1) {
        node->child[i]         = *bags[i]->begin();
        node->child[i]->parent = node;
        continue;
      }

      Point new_pos(node->pos);
      updateCenter(new_pos, i, radius);
      OctreeInternal* new_node = octreeInternalAlloc.allocate(1);
      octreeInternalAlloc.construct(new_node, new_pos, node);
      node->child[i] = new_node;

      galois::Task t = ctx.addTask1(
          BuildOctree(bodyPtrsAlloc, octreeInternalAlloc, bags[i]->begin(),
                      bags[i]->end(), new_node, radius, after));
      ctx.addDependence(t, after);
      ctx.markDeferred(after);
    }

    // Reorganize leaves to be denser up front
    int index = 0;
    for (int i = 0; i < 8; i++) {
      Octree* child = node->child[i];
      if (child == NULL)
        continue;

      if (index != i) {
        node->child[index] = child;
        node->child[i]     = NULL;
      }
      index++;
    }
  }
};

template <typename OutTy>
struct Sort {
  OctreeInternal* node;
  OutTy& out;

  Sort(OctreeInternal* n, OutTy& o) : node(n), out(o) {}

  //  void inspect() { } // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    for (int i = 0; i < 8; i++) {
      Octree* child = node->child[i];
      if (child == NULL)
        break;

      if (child->isLeaf()) {
        out.push_back(*static_cast<Body*>(child));
        continue;
      }
      ctx.addTask1(Sort<OutTy>(static_cast<OctreeInternal*>(child), out));
    }
  }

  void operator()(OctreeInternal* n) {
    for (int i = 0; i < 8; i++) {
      Octree* child = n->child[i];
      if (child == NULL)
        break;

      if (child->isLeaf()) {
        out.push_back(*static_cast<Body*>(child));
        continue;
      }
      (*this)(static_cast<OctreeInternal*>(child));
    }
  }
};

void updateForce(Point& acc, const Point& delta, double psq, double mass) {
  // Computing force += delta * mass * (|delta|^2 + eps^2)^{-3/2}
  psq += config.epssq;
  double idr   = 1 / sqrt((float)psq);
  double scale = mass * idr * idr * idr;

  for (int i = 0; i < 3; i++)
    acc[i] += delta[i] * scale;
}

void computeDelta(Point& p, const Body* body, const Octree* b) {
  for (int i = 0; i < 3; i++)
    p[i] = b->pos[i] - body->pos[i];
}

struct ComputeForceIndirect {
  Body* body;
  OctreeInternal* node;
  double dsq;

  ComputeForceIndirect(Body* b, OctreeInternal* n, double d)
      : body(b), node(n), dsq(d) {}

  //  void inspect() { } // XXX

  struct Frame {
    double dsq;
    OctreeInternal* node;
    Frame(OctreeInternal* _node, double _dsq) : dsq(_dsq), node(_node) {}
  };

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    std::deque<Frame*> stack;
    std::deque<Frame> backing;

    backing.push_back(Frame(node, dsq));
    stack.push_back(&backing.back());

    Point delta;
    while (!stack.empty()) {
      Frame f = *stack.back();
      stack.pop_back();

      computeDelta(delta, body, f.node);

      double psq = delta.dist2();
      // Node is far enough away, summarize contribution
      if (psq >= f.dsq) {
        updateForce(body->acc, delta, psq, f.node->mass);
        continue;
      }

      double new_dsq = f.dsq * 0.25;

      for (int i = 0; i < 8; i++) {
        Octree* next = f.node->child[i];
        if (next == NULL)
          break;
        if (body == next)
          continue;
        if (next->isLeaf()) {
          Point delta;
          computeDelta(delta, body, next);
          // body->mass is fine because every body has the same mass
          updateForce(body->acc, delta, delta.dist2(), body->mass);
        } else {
          backing.push_back(Frame(static_cast<OctreeInternal*>(next), new_dsq));
          stack.push_back(&backing.back());
        }
      }
    }
  }
};

struct ComputeForceBlockedContinuation {
  Body* body;
  OctreeInternal* node;
  Octree** next;
  double dsq;

  ComputeForceBlockedContinuation(Body* b, OctreeInternal* n, double d)
      : body(b), node(n), next(0), dsq(d) {
    down(node);
  }

  void down(OctreeInternal* n) {
    node = n;

    Point delta;
    computeDelta(delta, body, node);
    double psq = delta.dist2();

    if (psq >= dsq) {
      updateForce(body->acc, delta, psq, node->mass);
      up();
      return;
    }

    next = node->child;
    dsq  = dsq * 0.25;
  }

  void up() {
    while (true) {
      OctreeInternal* oldNode = node;

      node = static_cast<OctreeInternal*>(node->parent);
      if (!node)
        break;

      dsq = dsq * 4.0;

      for (int i = 0; i < 8; ++i) {
        assert(node->child[i]);
        if (node->child[i] == oldNode) {
          // always valid due to end of array element
          next = &node->child[i + 1];
          if (!next)
            break;
          return;
        }
      }
    }
  }

  // void inspect() { } // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    Point delta;
    for (int i = 0; i < blockSizeY && node != NULL; ++i) {
      Octree* n = *next;
      if (!n) {
        up();
      } else if (body == n) {
        ++next;
        --i; // don't include empty work in block factor
      } else if (n->isLeaf()) {
        Point delta;
        computeDelta(delta, body, n);
        // body->mass is fine because every body has the same mass
        updateForce(body->acc, delta, delta.dist2(), body->mass);
        ++next;
      } else {
        down(static_cast<OctreeInternal*>(n));
      }
    }

    if (node) {
      ctx.addTask1(*this);
    }
  }
};

struct ComputeForceBlocked {
  // typedef galois::gdeque<Body*,32> Bodies;
  typedef std::array<Body*, MaxBlockSizeX> Bodies;
  Bodies bodies;
  OctreeInternal* top;
  double dsq;
  int bmax;

  ComputeForceBlocked(OctreeInternal* n, double d) : top(n), dsq(d) {}

  //  void inspect() { } // XXX

  struct Frame {
    double dsq;
    Body* body;
    OctreeInternal* node;
    Frame(Body* b, OctreeInternal* _node, double _dsq)
        : dsq(_dsq), body(b), node(_node) {}
  };

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    typedef galois::gdeque<Frame> Stack;
    typedef std::array<Stack, MaxBlockSizeX> Stacks; // XXX

    Stacks stacks;

    Bodies::iterator ii = bodies.begin();
    for (int i = 0; i < bmax; ++i) {
      stacks[i].push_back(Frame(*ii++, top, dsq));
    }

    Point delta;
    while (true) {
      int numEmpty = 0;

      for (int bindex = 0; bindex < bmax; ++bindex) {
        if (stacks[bindex].empty()) {
          ++numEmpty;
          continue;
        }
        for (int yindex = 0; !stacks[bindex].empty() && yindex < blockSizeY;
             ++yindex) {
          Frame f = stacks[bindex].back();
          stacks[bindex].pop_back();

          computeDelta(delta, f.body, f.node);
          double psq = delta.dist2();
          if (psq >= f.dsq) {
            updateForce(f.body->acc, delta, psq, f.node->mass);
            continue;
          }

          double new_dsq = f.dsq * 0.25;

          for (int i = 0; i < 8; i++) {
            Octree* next = f.node->child[i];
            if (next == NULL)
              break;
            if (f.body == next)
              continue;
            if (next->isLeaf()) {
              Point delta;
              computeDelta(delta, f.body, next);
              // body->mass is fine because every body has the same mass
              updateForce(f.body->acc, delta, delta.dist2(), f.body->mass);
            } else {
              if (blockFIFO)
                stacks[bindex].push_front(
                    Frame(f.body, static_cast<OctreeInternal*>(next), new_dsq));
              else
                stacks[bindex].push_back(
                    Frame(f.body, static_cast<OctreeInternal*>(next), new_dsq));
            }
          }
        }
      }
      if (numEmpty == bmax)
        break;
    }
  }
};

struct ComputeForceOrig {
  Body* body;
  OctreeInternal* node;
  double dsq;

  ComputeForceOrig(Body* b, OctreeInternal* n, double d)
      : body(b), node(n), dsq(d) {}

  void inspect() {} // XXX

  struct Frame {
    double dsq;
    OctreeInternal* node;
    Frame(OctreeInternal* _node, double _dsq) : dsq(_dsq), node(_node) {}
  };

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    galois::gdeque<Frame> stack;
    stack.push_back(Frame(node, dsq));

    Point delta;
    while (!stack.empty()) {
      Frame f = stack.back();
      stack.pop_back();

      computeDelta(delta, body, f.node);

      double psq = delta.dist2();
      // Node is far enough away, summarize contribution
      if (psq >= f.dsq) {
        updateForce(body->acc, delta, psq, f.node->mass);
        continue;
      }

      double new_dsq = f.dsq * 0.25;

      for (int i = 0; i < 8; i++) {
        Octree* next = f.node->child[i];
        if (next == NULL)
          break;
        if (body == next)
          continue;
        if (next->isLeaf()) {
          Point delta;
          computeDelta(delta, body, next);
          // body->mass is fine because every body has the same mass
          updateForce(body->acc, delta, delta.dist2(), body->mass);
        } else {
          stack.push_back(Frame(static_cast<OctreeInternal*>(next), new_dsq));
        }
      }
    }
  }
};

struct FinishForceTree {
  typedef std::array<Point, 8> Accs;
  Accs accs;
  Point* parent;

  FinishForceTree(Point* p) : parent(p) {}

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    for (Accs::iterator ii = accs.begin(), ei = accs.end(); ii != ei; ++ii) {
      *parent += *ii;
    }
  }
};

struct ComputeForceTree {
  typedef std::list<galois::Task,
                    galois::PerIterAllocTy::rebind<galois::Task>::other>
      Tasks;

  Point* acc;
  Body* body;
  OctreeInternal* node;
  double dsq;
  galois::Task parent;

  ComputeForceTree(Point* a, Body* b, OctreeInternal* n, double d,
                   galois::Task p)
      : acc(a), body(b), node(n), dsq(d), parent(p) {}

  void inspect() {} // XXX

  template <typename PipelineTy>
  void recurse(galois::TaskContext<PipelineTy>& ctx, OctreeInternal* node,
               double dsq, int depth, Tasks& tasks) {
    Point delta;
    computeDelta(delta, body, node);

    double psq = delta.dist2();
    // Node is far enough away, summarize contribution
    if (psq >= dsq) {
      updateForce(*acc, delta, psq, node->mass);
      return;
    }

    galois::runtime::Task::GTask<FinishForceTree>* after = NULL;

    double new_dsq = dsq * 0.25;

    for (int i = 0; i < 8; i++) {
      Octree* next = node->child[i];
      if (next == NULL)
        break;
      if (body == next)
        continue;
      if (next->isLeaf()) {
        Point delta;
        computeDelta(delta, body, next);
        // body->mass is fine because every body has the same mass
        updateForce(*acc, delta, delta.dist2(), body->mass);
      } else if (depth > 0 || depth < 0) {
        recurse(ctx, static_cast<OctreeInternal*>(next), new_dsq, depth - 1,
                tasks);
      } else {
        if (!after) {
          after = ctx.addTask2(FinishForceTree(acc));
          if (parent)
            ctx.addDependence(after, parent);
          tasks.push_back(after);
        }

        galois::Task t = ctx.addTask1(ComputeForceTree(
            &after->task.accs[i], body, static_cast<OctreeInternal*>(next),
            new_dsq, after));
        ctx.addDependence(t, after);
        ctx.markDeferred(after);
      }
    }
  }

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    Tasks tasks(ctx.getPerIterAlloc());
    recurse(ctx, node, dsq, 3, tasks); // XXX
    // galois::Task prev = { };
    galois::Task prev = 0;
    // for (Tasks::reverse_iterator ii = tasks.rbegin(), ei = tasks.rend(); ii
    // != ei; ++ii) { //XXX
    for (Tasks::iterator ii = tasks.begin(), ei = tasks.end(); ii != ei; ++ii) {
      if (prev)
        ctx.addDependence(prev, *ii);
      prev = *ii;
    }
  }
};

struct ComputeForceUnroll {
  typedef int tt_needs_aborts;

  Body* body;
  OctreeInternal* node;
  double dsq;

  ComputeForceUnroll(Body* b, OctreeInternal* n, double d)
      : body(b), node(n), dsq(d) {}

  void inspect() {} // XXX

  template <typename PipelineTy>
  void recurse(galois::TaskContext<PipelineTy>& ctx, OctreeInternal* node,
               double dsq, int depth) {
    Point delta;
    computeDelta(delta, body, node);

    double psq = delta.dist2();
    // Node is far enough away, summarize contribution
    if (psq >= dsq) {
      updateForce(body->acc, delta, psq, node->mass);
      return;
    }

    double new_dsq = dsq * 0.25;

    for (int i = 0; i < 8; i++) {
      Octree* next = node->child[i];
      if (next == NULL)
        break;
      if (body == next)
        continue;
      if (next->isLeaf()) {
        Point delta;
        computeDelta(delta, body, next);
        // body->mass is fine because every body has the same mass
        updateForce(body->acc, delta, delta.dist2(), body->mass);
      } else if (depth > 0) {
        recurse(ctx, static_cast<OctreeInternal*>(next), new_dsq, depth - 1);
      } else {
        ctx.addTask1(ComputeForceUnroll(
            body, static_cast<OctreeInternal*>(next), new_dsq));
      }
    }
  }

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    galois::runtime::acquire(body, galois::MethodFlag::WRITE);
    recurse(ctx, node, dsq, 4);
  }
};

struct ComputeForceBase {
  typedef int tt_needs_aborts;

  Body* body;
  OctreeInternal* node;
  double dsq;

  ComputeForceBase(Body* b, OctreeInternal* n, double d)
      : body(b), node(n), dsq(d) {}

  void inspect() {} // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>& ctx) {
    Point delta;
    galois::runtime::acquire(body, galois::MethodFlag::WRITE);
    computeDelta(delta, body, node);

    double psq = delta.dist2();
    // Node is far enough away, summarize contribution
    if (psq >= dsq) {
      updateForce(body->acc, delta, psq, node->mass);
      return;
    }

    double new_dsq = dsq * 0.25;

    for (int i = 0; i < 8; i++) {
      Octree* next = node->child[i];
      if (next == NULL)
        break;
      if (body == next)
        continue;
      if (next->isLeaf()) {
        Point delta;
        computeDelta(delta, body, next);
        // body->mass is fine because every body has the same mass
        updateForce(body->acc, delta, delta.dist2(), body->mass);
      } else {
        ctx.addTask1(ComputeForceBase(body, static_cast<OctreeInternal*>(next),
                                      new_dsq));
      }
    }
  }
};

struct ComputeForceTaskOrig {
  struct Task {
    Body* body;
    Octree* other;
    Task(Body* b, Octree* o) : body(b), other(o) {}
  };

  struct Frame {
    double dsq;
    OctreeInternal* node;
    Frame(OctreeInternal* n, double d) : dsq(d), node(n) {}
  };

  template <typename IterTy, typename OutTy>
  void generateTasks(double dsq, OctreeInternal* top, IterTy ii, IterTy ei,
                     OutTy& out) {
    size_t dist  = std::distance(ii, ei);
    size_t block = (dist + (numThreads - 1)) / numThreads;
    size_t count = 0;

    for (; ii != ei; ++ii, ++count) {
      int tid    = count / block;
      Body* body = *ii;
      galois::gdeque<Frame> stack;
      stack.push_back(Frame(top, dsq));

      Point delta;
      while (!stack.empty()) {
        Frame f = stack.back();
        stack.pop_back();

        computeDelta(delta, body, f.node);
        double psq = delta.dist2();
        if (psq >= f.dsq) {
          out[tid].push_back(Task(body, f.node));
          continue;
        }

        double new_dsq = f.dsq * 0.25;

        for (int i = 0; i < 8; i++) {
          Octree* next = f.node->child[i];
          if (next == NULL)
            break;
          if (body == next)
            continue;
          if (next->isLeaf()) {
            out[tid].push_back(Task(body, next));
          } else {
            stack.push_back(Frame(static_cast<OctreeInternal*>(next), new_dsq));
          }
        }
      }
    }
  }

  void operator()(const Task& t) {
    Point delta;
    computeDelta(delta, t.body, t.other);
    updateForce(t.body->acc, delta, delta.dist2(), t.other->mass);
  }
};

struct ComputeForceTaskBlocked {
  struct Task {
    Body* body;
    Octree* other;
    Task(Body* b, Octree* o) : body(b), other(o) {}
  };

  struct Frame {
    double dsq;
    Body* body;
    OctreeInternal* node;
    int tid;
    Frame(int tid, Body* b, OctreeInternal* n, double d)
        : dsq(d), body(b), node(n), tid(tid) {}
  };

  template <typename IterTy, typename OutTy>
  void generateTasks(double dsq, OctreeInternal* top, IterTy ii, IterTy ei,
                     OutTy& out) {
    typedef galois::gdeque<Frame> Stack;
    typedef std::array<Stack, MaxBlockSizeX> Stacks;

    Stacks stacks;
    // stacks.resize(blockSizeX);

    size_t dist  = std::distance(ii, ei);
    size_t block = (dist + (numThreads - 1)) / numThreads;
    size_t count = 0;

    while (ii != ei) {
      int bmax;
      for (bmax = 0; ii != ei && bmax < blockSizeX; ++ii, ++count, ++bmax) {
        int tid    = count / block;
        Body* body = *ii;
        stacks[bmax].push_back(Frame(tid, body, top, dsq));
      }

      Point delta;
      while (true) {
        int numEmpty = 0;

        for (int bindex = 0; bindex < bmax; ++bindex) {
          if (stacks[bindex].empty()) {
            ++numEmpty;
            continue;
          }
          for (int yindex = 0; !stacks[bindex].empty() && yindex < blockSizeY;
               ++yindex) {
            Frame f = stacks[bindex].back();
            stacks[bindex].pop_back();

            computeDelta(delta, f.body, f.node);
            double psq = delta.dist2();
            if (psq >= f.dsq) {
              out[f.tid].push_back(Task(f.body, f.node));
              continue;
            }

            double new_dsq = f.dsq * 0.25;

            for (int i = 0; i < 8; i++) {
              Octree* next = f.node->child[i];
              if (next == NULL)
                break;
              if (f.body == next)
                continue;
              if (next->isLeaf()) {
                out[f.tid].push_back(Task(f.body, next));
              } else {
                if (blockFIFO)
                  stacks[bindex].push_front(
                      Frame(f.tid, f.body, static_cast<OctreeInternal*>(next),
                            new_dsq));
                else
                  stacks[bindex].push_back(
                      Frame(f.tid, f.body, static_cast<OctreeInternal*>(next),
                            new_dsq));
              }
            }
          }
        }
        if (numEmpty == bmax)
          break;
      }
    }
  }

  void operator()(const Task& t) {
    Point delta;
    computeDelta(delta, t.body, t.other);
    updateForce(t.body->acc, delta, delta.dist2(), t.other->mass);
  }
};

struct AdvanceBody {
  Body* body;
  Bodies* new_bodies;

  AdvanceBody(Body* b, Bodies* n) : body(b), new_bodies(n) {}

  void inspect() {} // XXX

  template <typename PipelineTy>
  void operator()(galois::TaskContext<PipelineTy>&) {
    for (int i = 0; i < 3; ++i) {
      body->vel[i] += (body->acc[i] - body->oldacc[i]) * config.dthf;
    }

    Point dvel(body->acc);
    dvel *= config.dthf;

    Point velh(body->vel);
    velh += dvel;

    for (int i = 0; i < 3; ++i) {
      body->pos[i] += velh[i] * config.dtime;
      body->vel[i]    = velh[i] + dvel[i];
      body->oldacc[i] = body->acc[i];
      body->acc[i]    = 0;
    }

    // new_bodies->push_back(*body); XXX
  }
};

// XXX use reduction support? or NUMA?
struct ReduceBoxes {
  BoundingBox& initial;

  ReduceBoxes(BoundingBox& _initial) : initial(_initial) {}

  void operator()(Body* b) { initial.merge(b->pos); }
};

double nextDouble() { return rand() / (double)RAND_MAX; }

/**
 * Generates random input according to the Plummer model, which is more
 * realistic but perhaps not so much so according to astrophysicists
 */
void generateInput(Bodies& bodies, int nbodies, int seed) {
  double v, sq, scale;
  Point p;
  double PI = boost::math::constants::pi<double>();

  srand(seed);

  double rsc = (3 * PI) / 16;
  double vsc = sqrt(1.0 / rsc);

  for (int body = 0; body < nbodies; body++) {
    double r = 1.0 / sqrt(pow(nextDouble() * 0.999, -2.0 / 3.0) - 1);
    do {
      for (int i = 0; i < 3; i++)
        p[i] = nextDouble() * 2.0 - 1.0;
      sq = p.x * p.x + p.y * p.y + p.z * p.z;
    } while (sq > 1.0);
    scale = rsc * r / sqrt(sq);

    Body b;
    b.mass = 1.0 / nbodies;
    for (int i = 0; i < 3; i++)
      b.pos[i] = p[i] * scale;

    do {
      p.x = nextDouble();
      p.y = nextDouble() * 0.1;
    } while (p.y > p.x * p.x * pow(1 - p.x * p.x, 3.5));
    v = p.x * sqrt(2.0 / sqrt(1 + r * r));
    do {
      for (int i = 0; i < 3; i++)
        p[i] = nextDouble() * 2.0 - 1.0;
      sq = p.x * p.x + p.y * p.y + p.z * p.z;
    } while (sq > 1.0);
    scale = vsc * v / sqrt(sq);
    for (int i = 0; i < 3; i++) {
      b.vel[i]    = p[i] * scale;
      b.acc[i]    = 0;
      b.oldacc[i] = 0;
    }

    bodies.push_back(b);
  }
}

template <typename T>
struct deref : public std::unary_function<T, T*> {
  T* operator()(T& item) const { return &item; }
};

template <typename ComputeForceTy, typename WLTy, typename Enable = void>
struct DoComputeForce {
  struct MakeComputeForce : public std::unary_function<Body*, ComputeForceTy> {
    OctreeInternal* top;
    double dsq;

    MakeComputeForce(OctreeInternal* t, double d) : top(t), dsq(d) {}

    ComputeForceTy operator()(Body* b) const {
      return ComputeForceTy(b, top, dsq);
    }
  };

  template <typename IterTy>
  void operator()(double dsq, OctreeInternal* top, IterTy begin, IterTy end) {
    typedef galois::Pipeline<WLTy, ComputeForceTy> P;
    galois::for_each_task<P>(
        boost::make_transform_iterator(begin, MakeComputeForce(top, dsq)),
        boost::make_transform_iterator(end, MakeComputeForce(top, dsq)),
        "MakeComputeForce");
  }
};

//! Returned blocked ranges for a fixed block size
template <typename IterTy>
class blocked_iterator : public std::iterator<std::forward_iterator_tag,
                                              std::pair<IterTy, IterTy>> {
  IterTy ii;
  IterTy ei;
  int block;

public:
  blocked_iterator() {}
  blocked_iterator(const IterTy& ii, const IterTy& ei, int block)
      : ii(ii), ei(ei), block(block) {}
  bool operator==(const blocked_iterator& rhs) const { return ii == rhs.ii; }
  bool operator!=(const blocked_iterator& rhs) const { return ii != rhs.ii; }
  std::pair<IterTy, IterTy> operator*() const {
    IterTy b(ii);
    IterTy e(ii);
    for (int i = 0; e != ei && i < block; ++i, ++e)
      ;
    return std::make_pair(b, e);
  }
  blocked_iterator& operator++() {
    for (int i = 0; ii != ei && i < block; ++i, ++ii)
      ;
    return *this;
  }
  blocked_iterator& operator++(int) {
    blocked_iterator tmp(*this);
    ++(*this);
    return tmp;
  }
};

template <typename WLTy, typename Enable>
struct DoComputeForce<ComputeForceBlocked, WLTy, Enable> {
  template <typename T>
  struct MakeComputeForce : public std::unary_function<T, ComputeForceBlocked> {
    OctreeInternal* top;
    double dsq;

    MakeComputeForce(OctreeInternal* t, double d) : top(t), dsq(d) {}

    ComputeForceBlocked operator()(const T& p) const {
      ComputeForceBlocked x(top, dsq);
      std::copy(p.first, p.second, x.bodies.begin());
      x.bmax = std::distance(p.first, p.second);
      return x;
    }
  };

  template <typename IterTy>
  void operator()(double dsq, OctreeInternal* top, IterTy begin, IterTy end) {
    typedef galois::Pipeline<WLTy, ComputeForceBlocked> P;
    typedef blocked_iterator<IterTy> BlockedIt;
    typedef typename std::iterator_traits<BlockedIt>::value_type value_type;

    galois::for_each_task<P>(
        boost::make_transform_iterator(BlockedIt(begin, end, blockSizeX),
                                       MakeComputeForce<value_type>(top, dsq)),
        boost::make_transform_iterator(BlockedIt(end, end, blockSizeX),
                                       MakeComputeForce<value_type>(top, dsq)),
        "MakeComputeForce");
  }
};

template <typename WLTy, typename Enable>
struct DoComputeForce<ComputeForceTree, WLTy, Enable> {
  struct MakeComputeForce
      : public std::unary_function<Body*, ComputeForceTree> {
    OctreeInternal* top;
    double dsq;

    MakeComputeForce(OctreeInternal* t, double d) : top(t), dsq(d) {}

    ComputeForceTree operator()(Body* b) const {
      return ComputeForceTree(&b->acc, b, top, dsq, 0);
    }
  };

  template <typename IterTy>
  void operator()(double dsq, OctreeInternal* top, IterTy begin, IterTy end) {
    typedef galois::Pipeline<WLTy, ComputeForceTree, FinishForceTree> P;
    galois::for_each_task<P>(
        boost::make_transform_iterator(begin, MakeComputeForce(top, dsq)),
        boost::make_transform_iterator(end, MakeComputeForce(top, dsq)),
        "MakeComputeForce");
  }
};

//! Some template meta programming
template <typename T>
struct has_task {
  typedef char yes[1];
  typedef char no[2];
  template <typename U>
  static yes& test(typename U::Task*);
  template <typename>
  static no& test(...);
  static const bool value = sizeof(test<T>(0)) == sizeof(yes);
};

//! Simple iterator that returns iterators rather than values
template <typename IterTy>
class passthrough : public std::iterator<std::forward_iterator_tag, IterTy> {
  IterTy ii;

public:
  passthrough() {}
  passthrough(const IterTy& ii) : ii(ii) {}
  bool operator==(const passthrough& rhs) const { return ii == rhs.ii; }
  bool operator!=(const passthrough& rhs) const { return ii != rhs.ii; }
  const IterTy& operator*() const { return ii; }
  IterTy& operator*() { return ii; }
  passthrough& operator++() {
    ++ii;
    return *this;
  }
  passthrough operator++(int) {
    passthrough tmp(*this);
    ++ii;
    return tmp;
  }
};

template <typename T>
double refineDelta(const T& a, const T& b) {
  double bdiff =
      reinterpret_cast<intptr_t>(a.body) - reinterpret_cast<intptr_t>(b.body);
  double odiff =
      reinterpret_cast<intptr_t>(a.other) - reinterpret_cast<intptr_t>(b.other);
  return bdiff * bdiff + odiff * odiff;
}

template <typename IterTy>
struct Refine {
  IterTy ii;
  IterTy ei;
  size_t dist;

  bool improvement(const IterTy& a, const IterTy& b) {
    IterTy anext = skip(a, 1);
    IterTy bnext = skip(b, 1);
    IterTy aprev = skip(a, -1);
    IterTy bprev = skip(b, -1);

    return refineDelta(*aprev, *b) + refineDelta(*b, *anext) <
               refineDelta(*aprev, *a) + refineDelta(*a, *anext) &&
           refineDelta(*bprev, *a) + refineDelta(*a, *bnext) <
               refineDelta(*bprev, *b) + refineDelta(*b, *bnext);
    // return refineDelta(*b, *anext) < refineDelta(*a, *anext) // XXX
    //  && refineDelta(*a, *bnext) < refineDelta(*b, *bnext);
  }

  IterTy skip(IterTy c, int n) {
    if (n > 0) {
      int remaining = std::distance(c, ei);
      if (remaining > n) {
        std::advance(c, n);
      } else {
        c = ii;
        std::advance(c, n - remaining);
      }
    } else {
      int remaining = std::distance(ii, c);
      if (remaining > -n) {
        std::advance(c, n);
      } else {
        c = ei;
        std::advance(c, n + remaining);
      }
    }

    return c;
  }

  Refine(const IterTy& ii, const IterTy& ei, size_t dist)
      : ii(ii), ei(ei), dist(dist) {}

  void operator()(const IterTy& cur, galois::UserContext<IterTy>&) {
    IterTy next = skip(cur, dist);
    if (improvement(cur, next)) {
      galois::runtime::acquire((*cur).body, galois::MethodFlag::WRITE);
      galois::runtime::acquire((*next).body, galois::MethodFlag::WRITE);
      std::swap(*cur, *next);
    }
  }
};

template <typename IterTy>
struct ComputeRefineDelta {
  typedef typename std::iterator_traits<IterTy>::value_type value_type;
  IterTy ei;
  ComputeRefineDelta(const IterTy& ei) : ei(ei) {}
  double operator()(IterTy cur) const {
    const value_type& v = *cur;
    std::advance(cur, 1);
    if (cur == ei)
      return 0.0;
    double retval = refineDelta(v, *cur);
    assert(retval >= 0);
    return retval;
  }
};

template <typename OutTy>
void refine(OutTy& out) {
  typedef typename OutTy::iterator Iter;
  double newDelta = galois::ParallelSTL::map_reduce(
      passthrough<Iter>(out.begin()), passthrough<Iter>(out.end()),
      ComputeRefineDelta<Iter>(out.end()), 0.0, std::plus<double>());
  double oldDelta = 0;
  // size_t dist = out.size();

  while (true) {
    std::cout << "DELTA: " << oldDelta << " " << newDelta << " "
              << (oldDelta - newDelta) / oldDelta << "\n";
    if (oldDelta != 0 && newDelta > oldDelta)
      break;
    if (oldDelta != 0 && (oldDelta - newDelta) / oldDelta < 0.01)
      break;

    for (size_t d = out.size() >> 1; d != 0; d = d >> 1) {
      // for (size_t d = 1024; d != 0; d = d >> 1) { // XXX
      galois::for_each(passthrough<Iter>(out.begin()),
                       passthrough<Iter>(out.end()),
                       Refine<Iter>(out.begin(), out.end(), d));
    }
    oldDelta = newDelta;
    newDelta = galois::ParallelSTL::map_reduce(
        passthrough<Iter>(out.begin()), passthrough<Iter>(out.end()),
        ComputeRefineDelta<Iter>(out.end()), 0.0, std::plus<double>());
  }
}

template <typename ComputeForceTy, typename WLTy>
struct DoComputeForce<
    ComputeForceTy, WLTy,
    typename boost::enable_if<has_task<ComputeForceTy>>::type> {

  template <typename OutsTy>
  struct Wrapper {
    ComputeForceTy& w;
    OutsTy& outs;
    Wrapper(ComputeForceTy& w, OutsTy& outs) : w(w), outs(outs) {}

    void operator()(unsigned tid, unsigned) {
      std::for_each(outs[tid].begin(), outs[tid].end(), w);
    }
  };

  template <typename IterTy>
  void operator()(double dsq, OctreeInternal* top, IterTy begin, IterTy end) {
    // typedef galois::gdeque<typename ComputeForceTy::Task> Deque; // XXX
    typedef std::vector<typename ComputeForceTy::Task> Deque;
    typedef std::vector<Deque> Outs;

    ComputeForceTy W;
    Outs outs;

    outs.resize(numThreads);

    galois::StatTimer genTime("GenerateTasksTime");
    genTime.start();
    W.generateTasks(dsq, top, begin, end, outs);
    genTime.stop();

    if (useTaskRefine) {
      galois::StatTimer refineTime("RefineTasksTime");
      refineTime.start();
      for (int i = 0; i < numThreads; ++i) {
        std::cout << "Refine " << i << "\n";
        refine(outs[i]);
      }
      refineTime.stop();
    }

    galois::StatTimer computeTime("ParallelComputeForceTime");
    computeTime.start();
    galois::on_each(Wrapper<Outs>(W, outs),
                    galois::loopname("MakeComputeForce"));
    computeTime.stop();
  }
};

struct MakeAdvanceBody : public std::unary_function<Body*, AdvanceBody> {
  Bodies* new_bodies;

  MakeAdvanceBody(Bodies* n) : new_bodies(n) {}

  AdvanceBody operator()(Body* b) const { return AdvanceBody(b, new_bodies); }
};

struct CheckAllPairs {
  Bodies& bodies;

  CheckAllPairs(Bodies& b) : bodies(b) {}

  double operator()(const Body& body) const {
    const Body* me = &body;
    Point acc;
    for (Bodies::iterator ii = bodies.begin(), ei = bodies.end(); ii != ei;
         ++ii) {
      Body* b = &*ii;
      if (me == b)
        continue;

      Point delta;
      computeDelta(delta, me, b);
      double psq = delta.dist2();
      updateForce(acc, delta, psq, b->mass);
    }

    double dist2 = acc.dist2();
    for (int i = 0; i < 3; ++i)
      acc[i] -= me->acc[i];
    double retval = acc.dist2() / dist2;
    return retval;
  }
};

double checkAllPairs(Bodies& bodies, int N) {
  Bodies::iterator end(bodies.begin());
  std::advance(end, N);

  return galois::ParallelSTL::map_reduce(bodies.begin(), end,
                                         CheckAllPairs(bodies), 0.0,
                                         std::plus<double>()) /
         N;
}

void sort(Bodies& bodies) {
  // XXX resort
  // XXX resort better
  BodyPtrs ptrs;
  std::copy(boost::make_transform_iterator(bodies.begin(), deref<Body>()),
            boost::make_transform_iterator(bodies.end(), deref<Body>()),
            std::back_inserter(ptrs));

  BoundingBox box;
  ReduceBoxes reduceBoxes(box);
  std::for_each(ptrs.begin(), ptrs.end(), ReduceBoxes(box)); // XXX

  BuildOctree::BodyPtrsAlloc bodyPtrsAlloc;
  BuildOctree::OctreeInternalAlloc octreeInternalAlloc;
  OctreeInternal* top = octreeInternalAlloc.allocate(1);
  octreeInternalAlloc.construct(
      top, OctreeInternal(box.center(), static_cast<Octree*>(0)));

  // XXX
  typedef galois::Pipeline<galois::worklists::PerSocketChunkLIFO<>, BuildOctree,
                           ComputeCenterOfMass>
      P1;
  galois::for_each_task<P1>(BuildOctree(bodyPtrsAlloc, octreeInternalAlloc,
                                        ptrs.begin(), ptrs.end(), top,
                                        box.radius(), 0),
                            "Sort1");

  if (false) {
    typedef galois::InsertBag<Body> Bag;
    typedef galois::Pipeline<galois::worklists::PerSocketChunkFIFO<>, Sort<Bag>>
        P2;
    Bag bag;
    galois::for_each_task<P2>(Sort<Bag>(top, bag), "Sort2");
    // delete top;
    bodies.clear();
    std::copy(bag.begin(), bag.end(), std::back_inserter(bodies));
  } else {
    typedef galois::gdeque<Body> Bag;
    Bag bag;
    Sort<Bag> sorter(top, bag);
    sorter(top);
    // delete top;
    bodies.clear();
    std::copy(bag.begin(), bag.end(), std::back_inserter(bodies));
  }
}

void run(Bodies& bodies) {
  Bodies new_bodies;
  BodyPtrs ptrs;

  std::copy(boost::make_transform_iterator(bodies.begin(), deref<Body>()),
            boost::make_transform_iterator(bodies.end(), deref<Body>()),
            std::back_inserter(ptrs));

  for (int step = 0; step < ntimesteps; step++) {
    BuildOctree::BodyPtrsAlloc bodyPtrsAlloc;
    BuildOctree::OctreeInternalAlloc octreeInternalAlloc;

    BoundingBox box;
    ReduceBoxes reduceBoxes(box);
    std::for_each(ptrs.begin(), ptrs.end(), ReduceBoxes(box)); // XXX

    OctreeInternal* top = octreeInternalAlloc.allocate(1);
    octreeInternalAlloc.construct(top, box.center(), static_cast<Octree*>(0));

    galois::StatTimer Toctree("BuildOctreeTime");
    Toctree.start();
    if (useCilkTreeBuild) {
      BuildOctreeCilk c(bodyPtrsAlloc, octreeInternalAlloc);
      c.buildOctree(ptrs.begin(), ptrs.end(), top, box.radius());
    } else if (allocateFIFO) {
      typedef galois::Pipeline<galois::worklists::PerSocketChunkFIFO<>,
                               BuildOctree, ComputeCenterOfMass>
          P1;
      galois::for_each_task<P1>(BuildOctree(bodyPtrsAlloc, octreeInternalAlloc,
                                            ptrs.begin(), ptrs.end(), top,
                                            box.radius(), 0),
                                "BuildOctree");
    } else {
      typedef galois::Pipeline<galois::worklists::PerSocketChunkLIFO<>,
                               BuildOctree, ComputeCenterOfMass>
          P1;
      galois::for_each_task<P1>(BuildOctree(bodyPtrsAlloc, octreeInternalAlloc,
                                            ptrs.begin(), ptrs.end(), top,
                                            box.radius(), 0),
                                "BuildOctree");
    }
    Toctree.stop();

    typedef galois::worklists::PerSocketChunkFIFO<> FIFO;
    typedef galois::worklists::PerSocketChunkLIFO<> LIFO;
    typedef galois::worklists::LocalWorklist<galois::worklists::GFIFO<>>
        LocalFIFO;

    galois::StatTimer T("ComputeForceTime");
    T.start();
    // XXX Use octree to align bodies in next version of bodies vector
    double dsq = box.diameter() * box.diameter() * config.itolsq;
    switch (algo) {
    case algoBase:
      DoComputeForce<ComputeForceBase, FIFO>()(dsq, top, ptrs.begin(),
                                               ptrs.end());
      break;
    case algoOrig:
      DoComputeForce<ComputeForceOrig, FIFO>()(dsq, top, ptrs.begin(),
                                               ptrs.end());
      break;
    case algoBlocked:
      DoComputeForce<ComputeForceBlocked, LocalFIFO>()(dsq, top, ptrs.begin(),
                                                       ptrs.end());
      break;
    case algoBlockedC:
      DoComputeForce<ComputeForceBlockedContinuation, FIFO>()(
          dsq, top, ptrs.begin(), ptrs.end());
      break;
    case algoIndirect:
      DoComputeForce<ComputeForceIndirect, FIFO>()(dsq, top, ptrs.begin(),
                                                   ptrs.end());
      break;
    case algoUnroll:
      DoComputeForce<ComputeForceUnroll, FIFO>()(dsq, top, ptrs.begin(),
                                                 ptrs.end());
      break;
    case algoTree:
      DoComputeForce<ComputeForceTree, FIFO>()(dsq, top, ptrs.begin(),
                                               ptrs.end());
      break;
    case algoTaskOrig:
      DoComputeForce<ComputeForceTaskOrig, FIFO>()(dsq, top, ptrs.begin(),
                                                   ptrs.end());
      break;
    case algoTaskBlocked:
      DoComputeForce<ComputeForceTaskBlocked, FIFO>()(dsq, top, ptrs.begin(),
                                                      ptrs.end());
      break;
    default:
      abort();
    }
    T.stop();

    if (!skipVerify) {
      std::cout << "MSE (sampled) "
                << checkAllPairs(bodies, std::min((int)nbodies, 100)) << "\n";
    }

    typedef galois::Pipeline<galois::worklists::PerSocketChunkFIFO<>,
                             AdvanceBody>
        P3;
    galois::for_each_task<P3>(boost::make_transform_iterator(
                                  ptrs.begin(), MakeAdvanceBody(&new_bodies)),
                              boost::make_transform_iterator(
                                  ptrs.end(), MakeAdvanceBody(&new_bodies)),
                              "AdvanceBody");

    std::cout << "Timestep " << step << " Center of Mass = ";
    std::ios::fmtflags flags =
        std::cout.setf(std::ios::showpos | std::ios::right |
                       std::ios::scientific | std::ios::showpoint);
    std::cout << top->pos;
    std::cout.flags(flags);
    std::cout << "\n";

    // XXX some simple parallel deallocation too using a counter just to
    // eliminate empty pages should cover nested and streaming case
    // delete top;
    // bodies.clear(); XXX
    // ptrs.clear();
    // boost::swap(bodies, new_bodies);
  }
}

int main(int argc, char** argv) {
  galois::StatManager M;
  LonestarStart(argc, argv, name, desc, url);

  // FixThreads fixThreads;
  // tbb::task_scheduler_init
  // iii(tbb::task_scheduler_init::default_num_threads()); XXX

  if (blockSizeX > MaxBlockSizeX) {
    std::cerr << "blockSizeX too large\n";
    abort();
  }

  std::cerr << "configuration: " << nbodies << " bodies, " << ntimesteps
            << " time steps" << std::endl
            << std::endl;

  Bodies bodies;
  generateInput(bodies, nbodies, seed);
  sort(bodies);

  galois::reportPageAlloc("MemPre");
  galois::StatTimer T;
  T.start();
  run(bodies);
  T.stop();
  galois::reportPageAlloc("MemPost");
}


text:
#define BOOST_TEST_MAIN

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

max_stars_repo_name: grahambrooks/jx

max_issues_repo_name: grahambrooks/jx

max_forks_repo_name: grahambrooks/jx