/* Greg Anderson
 *
 * Definition of a bounded powerset domain for some underlying domain in the
 * ELINA abstract interpretation library.
 */

#include <zonotope.h>
#include <elina_box_internal.h>

#include <cstdlib>
#include <iostream>
#include <Eigen/Dense>
#include <elina_box_internal.h>

#include "powerset.hpp"

Abstract0::Abstract0(elina_manager_t* m, elina_abstract0_t* a):
  man{m}, value{a}, center_computed{false} {}

Abstract0::Abstract0(const Abstract0& other):
  man{other.man}, value{elina_abstract0_copy(other.man, other.value)},
  center{other.center}, center_computed{other.center_computed} {}

Abstract0::Abstract0(Abstract0&& other):
  man{other.man}, value{other.value}, center{other.center},
  center_computed{other.center_computed} {
  other.value = nullptr;
}

Abstract0::~Abstract0() {
  if (value != nullptr) {
    elina_abstract0_free(man, value);
  }
}

Abstract0& Abstract0::operator=(const Abstract0& other) {
  this->man = other.man;
  this->value = elina_abstract0_copy(other.man, other.value);
  this->center = other.center;
  this->center_computed = other.center_computed;
  return *this;
}

Abstract0& Abstract0::operator=(Abstract0&& other) {
  this->man = other.man;
  this->value = other.value;
  other.value = nullptr;
  this->center = other.center;
  this->center_computed = other.center_computed;
  return *this;
}

Eigen::VectorXd compute_center(elina_manager_t* man, elina_abstract0_t* abs) {
  elina_interval_t** itv;
  itv = elina_abstract0_to_box(man, abs);
  int dims = elina_abstract0_dimension(man, abs).realdim;
  Eigen::VectorXd center(dims);
  for (int i = 0; i < dims; i++) {
    double l, u;
    elina_double_set_scalar(&l, itv[i]->inf, MPFR_RNDN);
    elina_double_set_scalar(&u, itv[i]->sup, MPFR_RNDN);
    center(i) = (l + u) / 2.0;
  }
  return center;
}

Powerset::Powerset(const Powerset& other) {
  disjuncts = other.disjuncts;
  size = other.size;
}

Powerset::Powerset(elina_manager_t* m, elina_abstract0_t* a, int s) {
  size = s;
  disjuncts = std::vector<std::shared_ptr<Abstract0>>();
  disjuncts.push_back(std::make_shared<Abstract0>(m, a));
}

Powerset::Powerset(std::vector<std::shared_ptr<Abstract0>>& ds, int s) {
  size = s;
  disjuncts = ds;
}

Powerset::Powerset(std::vector<std::shared_ptr<Abstract0>>& ds,
    std::vector<Eigen::VectorXd>& cs, int s) {
  size = s;
  disjuncts = std::vector<std::shared_ptr<Abstract0>>(ds);
}

Powerset Powerset::assign_linexpr_array(elina_dim_t* dims,
    elina_linexpr0_t** update, unsigned int size, unsigned int s) const {
  std::vector<std::shared_ptr<Abstract0>> ds;
  for (std::shared_ptr<Abstract0> it : this->disjuncts) {
    elina_abstract0_t* it_dim;
    size_t num_dims = elina_abstract0_dimension(it->man, it->value).realdim;
    if (s > num_dims) {
      // If the output size is greater than the input size, then we need to
      // add dimensions to the input abstract value.
      elina_dimchange_t* dc = elina_dimchange_alloc(0, s - num_dims);
      for (unsigned int i = 0; i < s - num_dims; i++) {
        dc->dim[i] = num_dims;
      }
      it_dim = elina_abstract0_add_dimensions(it->man, false, it->value, dc, false);
      elina_dimchange_free(dc);
    } else {
      it_dim = elina_abstract0_copy(it->man, it->value);
    }

    elina_abstract0_t* abs = elina_abstract0_assign_linexpr_array(
        it->man, false, it_dim, dims, update, size, NULL);
    elina_abstract0_free(it->man, it_dim);
    bool bot = elina_abstract0_is_bottom(it->man, abs);
    if (bot) {
      // If this value is bottom we don't need to add it back to the powerset.
      elina_abstract0_free(it->man, abs);
      continue;
    }

    if (num_dims > s) {
      // If the input size is greater than the output size, then we need to
      // remove excess dimensions here.
      elina_dimchange_t* dc = elina_dimchange_alloc(0, num_dims - s);
      for (unsigned int i = 0; i < num_dims - s; i++) {
        dc->dim[i] = s + i;
      }
      elina_abstract0_t* abs2 = elina_abstract0_remove_dimensions(it->man, false, abs, dc);
      elina_abstract0_free(it->man, abs);
      abs = abs2;
      elina_dimchange_free(dc);
    }

    ds.push_back(std::make_shared<Abstract0>(it->man, abs));
  }

  return Powerset(ds, this->size);
}

Powerset Powerset::meet_lincons_array(elina_lincons0_array_t* cons) const {
  std::vector<std::shared_ptr<Abstract0>> ds;
  for (std::shared_ptr<Abstract0> it : this->disjuncts) {
    elina_abstract0_t* abs = elina_abstract0_meet_lincons_array(
        it->man, false, it->value, cons);
    bool bot = elina_abstract0_is_bottom(it->man, abs);
    // If this value is bottom it doesn't affect the powerset.
    if (!bot) {
      ds.push_back(std::make_shared<Abstract0>(it->man, abs));
    } else {
      elina_abstract0_free(it->man, abs);
    }
  }
  return Powerset(ds, this->size);
}

Powerset Powerset::permute_dimensions(elina_dimperm_t* dp) const {
  std::vector<std::shared_ptr<Abstract0>> ds;
  for (std::shared_ptr<Abstract0> it : this->disjuncts) {
    elina_abstract0_t* abs = elina_abstract0_permute_dimensions(
        it->man, false, it->value, dp);
    ds.push_back(std::make_shared<Abstract0>(it->man, abs));
  }
  Powerset p(ds, this->size);
  // Permute the centers as well so we don't need to recompute them.
  if (disjuncts.size() > 0) {
    int dims = elina_abstract0_dimension(disjuncts[0]->man, disjuncts[0]->value).realdim;
    Eigen::VectorXi perm(dims);
    for (int i = 0; i < dims; i++) {
      perm(i) = dp->dim[i];
    }
    Eigen::PermutationMatrix<Eigen::Dynamic> pm(perm);
    for (unsigned int i = 0; i < disjuncts.size(); i++) {
      if (disjuncts[i]->center_computed) {
        p.disjuncts[i]->center_computed = true;
        p.disjuncts[i]->center = pm * disjuncts[i]->center;
      }
    }
  }
  return p;
}

Powerset Powerset::remove_dimensions(elina_dimchange_t* dc) const {
  std::vector<std::shared_ptr<Abstract0>> ds;
  for (std::shared_ptr<Abstract0> it : this->disjuncts) {
    elina_abstract0_t* abs = elina_abstract0_remove_dimensions(
        it->man, false, it->value, dc);
    bool bot = elina_abstract0_is_bottom(it->man, abs);
    // bottom elements don't need to be added to this powerset.
    if (!bot) {
      ds.push_back(std::make_shared<Abstract0>(it->man, abs));
    } else {
      elina_abstract0_free(it->man, abs);
    }
  }
  Powerset p(ds, this->size);
  // Update centers
  if (disjuncts.size() > 0) {
    unsigned int dims = elina_abstract0_dimension(
        disjuncts[0]->man, disjuncts[0]->value).realdim;
    unsigned int out_dims = dims - dc->realdim;
    for (unsigned int i = 0; i < disjuncts.size(); i++) {
      if (disjuncts[i]->center_computed) {
        p.disjuncts[i]->center_computed = true;
        Eigen::VectorXd c(out_dims);
        int dc_ind = 0;
        int c_ind = 0;
        for (unsigned int j = 0; j < dims; j++) {
          if (dc->dim[dc_ind] == j) {
            dc_ind++;
            continue;
          }
          c(c_ind) = disjuncts[i]->center(j);
          c_ind++;
        }
      }
    }
  }
  return p;
}

Powerset Powerset::affine(const Eigen::MatrixXd& m, const Eigen::VectorXd& b) const {
  int in_size = m.cols();
  int out_size = m.rows();

  // Create an elina linexpr array representing this update
  elina_dim_t* dims = (elina_dim_t*) malloc(out_size * sizeof(elina_dim_t));
  elina_linexpr0_t** update = (elina_linexpr0_t**) malloc(out_size *
      sizeof(elina_linexpr0_t*));
  for (int j = 0; j < out_size; j++) {
    dims[j] = j;
    update[j] = elina_linexpr0_alloc(ELINA_LINEXPR_DENSE, in_size);
    for (int k = 0; k < in_size; k++) {
      elina_linexpr0_set_coeff_scalar_double(update[j], k, m(j,k));
    }
    elina_linexpr0_set_cst_scalar_double(update[j], b(j));
  }

  Powerset z = assign_linexpr_array(dims, update, out_size, out_size);

  free(dims);
  for (int j = 0; j < out_size; j++) {
    elina_linexpr0_free(update[j]);
  }
  free(update);

  return z;
}

Powerset Powerset::relu() const {
  Powerset z = *this;
  size_t num_dims = elina_abstract0_dimension(
      disjuncts[0]->man, disjuncts[0]->value).realdim;
  for (unsigned int i = 0; i < num_dims; i++) {
    // Create two linear constraints, so that we can meet z with
    // x_i <= 0 and x_i >= 0.
    elina_linexpr0_t* lt0_le = elina_linexpr0_alloc(ELINA_LINEXPR_SPARSE, 1);
    elina_linexpr0_t* gt0_le = elina_linexpr0_alloc(ELINA_LINEXPR_SPARSE, 1);
    elina_linexpr0_set_coeff_scalar_double(lt0_le, i, -1.0);
    elina_linexpr0_set_coeff_scalar_double(gt0_le, i, 1.0);

    elina_lincons0_array_t lt0 = elina_lincons0_array_make(1);
    elina_lincons0_array_t gt0 = elina_lincons0_array_make(1);
    lt0.p[0].constyp = ELINA_CONS_SUPEQ;
    lt0.p[0].linexpr0 = lt0_le;
    gt0.p[0].constyp = ELINA_CONS_SUP;
    gt0.p[0].linexpr0 = gt0_le;

    // zlt = z `meet` x_i <= 0
    Powerset zlt = z.meet_lincons_array(&lt0);
    // z = z `meet` x_i >= 0
    z = z.meet_lincons_array(&gt0);

    // Assign x_i = 0 in zlt
    elina_linexpr0_t* zero = elina_linexpr0_alloc(ELINA_LINEXPR_SPARSE, 0);
    elina_linexpr0_set_cst_scalar_double(zero, 0.0);
    elina_dim_t dim = i;
    zlt = zlt.assign_linexpr_array(&dim, &zero, 1, num_dims);

    // Join z with the modified zlt
    z = z.join(zlt);

    elina_linexpr0_free(zero);
    elina_lincons0_array_clear(&lt0);
    elina_lincons0_array_clear(&gt0);
  }

  return z;
}

Powerset Powerset::join(const Powerset& other) const {
  if (this->disjuncts.size() == 0) {
    return other;
  } else if (other.disjuncts.size() == 0) {
    return *this;
  }
  std::vector<std::shared_ptr<Abstract0>> ds(disjuncts);
  ds.insert(ds.end(), other.disjuncts.begin(), other.disjuncts.end());
  // ds now holds all of the disjuncts from both powersets
  unsigned int s = std::max(size, other.size);
  while (ds.size() > s) {
    // Find the two disjuncts whose centers are closes to each other
    int best_i = 0, best_j = 1;
    if (s > 1) {
      if (!ds[0]->center_computed) {
        ds[0]->center = compute_center(ds[0]->man, ds[0]->value);
      }
      if (!ds[1]->center_computed) {
        ds[1]->center = compute_center(ds[1]->man, ds[1]->value);
      }
      double best_dist = (ds[0]->center - ds[1]->center).norm();
      for (unsigned int i = 0; i < ds.size() - 1; i++) {
        if (!ds[i]->center_computed) {
          ds[i]->center = compute_center(ds[i]->man, ds[i]->value);
        }
        for (unsigned int j = i+1; j < ds.size(); j++) {
          if (!ds[j]->center_computed) {
            ds[j]->center = compute_center(ds[j]->man, ds[j]->value);
          }
          double dist = (ds[i]->center - ds[j]->center).lpNorm<Eigen::Infinity>();
          if (dist < best_dist) {
            best_i = i;
            best_j = j;
            best_dist = dist;
          }
        }
      }
    }
    // Join those two disjuncts
    elina_abstract0_t* n = elina_abstract0_join(ds[best_i]->man, false,
        ds[best_i]->value, ds[best_j]->value);
    // j > i so we don't need to worry about messing up indices if we erase
    // j first
    ds.erase(ds.begin() + best_j);
    ds.erase(ds.begin() + best_i);
    ds.push_back(std::make_shared<Abstract0>(ds[best_i]->man, n));
  }

  return Powerset(ds, s);
}

bool Powerset::is_bottom() const {
  bool bottom = true;
  for (std::shared_ptr<Abstract0> it : this->disjuncts) {
    if (!elina_abstract0_is_bottom(it->man, it->value)) {
      bottom = false;
      break;
    }
  }
  // Note that if disjuncts is empty then this powerset is bottom.

  return bottom;
}

ssize_t Powerset::dims() const {
  if (this->disjuncts.size() > 0)
    return elina_abstract0_dimension(
        this->disjuncts[0]->man, this->disjuncts[0]->value).realdim;
  return 0;
}

elina_interval_t** Powerset::bounding_box() const {
  elina_interval_t** itvs[disjuncts.size()];
  for (unsigned int i = 0; i < disjuncts.size(); i++) {
    itvs[i] = elina_abstract0_to_box(disjuncts[i]->man, disjuncts[i]->value);
  }
  size_t dims = elina_abstract0_dimension(disjuncts[0]->man, disjuncts[0]->value).realdim;
  elina_interval_t** ret = (elina_interval_t**) malloc(dims * sizeof(elina_interval_t*));
  for (unsigned int i = 0; i < dims; i++) {
    ret[i] = elina_interval_alloc();
    double lowest = std::numeric_limits<double>::max();
    double highest = -std::numeric_limits<double>::max();
    for (unsigned int j = 0; j < disjuncts.size(); j++) {
      double l, u;
      elina_double_set_scalar(&l, itvs[j][i]->inf, MPFR_RNDN);
      elina_double_set_scalar(&u, itvs[j][i]->sup, MPFR_RNDN);
      if (l < lowest) {
        lowest = l;
      }
      if (u > highest) {
        highest = u;
      }
    }
    elina_interval_set_double(ret[i], lowest, highest);
  }
  for (unsigned int i = 0; i < disjuncts.size(); i++) {
    for (unsigned int j = 0; j < dims; j++) {
      elina_interval_free(itvs[i][j]);
    }
    free(itvs[i]);
  }
  return ret;
}

void Powerset::print_bounding_box() const {
  std::cout.precision(std::numeric_limits<double>::max_digits10);
  elina_interval_t** box = bounding_box();
  for (int i = 0; i < dims(); i++) {
    double l, u;
    elina_double_set_scalar(&l, box[i]->inf, MPFR_RNDN);
    elina_double_set_scalar(&u, box[i]->sup, MPFR_RNDN);
    std::cout << "[" << l << ", " << u << "]" << std::endl;
    elina_interval_free(box[i]);
  }
  free(box);
}

Powerset& Powerset::operator=(const Powerset& other) {
  this->size = other.size;
  disjuncts = other.disjuncts;
  return *this;
}


#pragma once
//=====================================================================//
/*!	@file
	@brief	各種頂点の定義 @n
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2017, 2018 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/glfw_app/blob/master/LICENSE
*/
//=====================================================================//
#include <vector>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <cmath>
#include <cfloat>
#include <cstdint>

namespace vtx {

// note: 通常のマシンでは、float 型より double 型の方が高速だが、リソースが限られた
//       マシンでは、その傾向は逆転する。
//       RXv2 コアでは、float 型は高速だが double 型は遅い。
//       RXv2 コアには、「fsqrt」（float 型専用の平方根命令がある）
#if defined(SIG_RX24T) || defined(SIG_RX64M) || defined(SIG_RX71M) || defined(SIG_RX65N) || defined(SIG_RX66T) || defined(SIG_RX72M) || defined(SIG_RX72T) || defined(SIG_RX72N)
	inline float fsqrt(float x)
	{
		__asm __volatile(
			"fsqrt %0, %0\n" \
			: "+r"(x) \
		);
		return x;
	}
#else
	inline float fsqrt(float x) { return std::sqrt(x); }
#endif

	template <typename T>
	T get_pi() { return static_cast<T>(3.1415926535897932384626433832795); }

	const float  deg2rad_f_ = get_pi<float>() / 180.0f;		///< DEG -> RAD の変換定数(float)
	const float  radian_f_  = 2.0f * get_pi<float>();		///< RADIAN のπ変換定数(float)
	const double deg2rad_d_ = get_pi<double>() / 180.0;		///< DEG -> RAD の変換定数(double)
	const double radian_d_  = 2.0 * get_pi<double>();		///< RADIAN のπ変換定数(double)

	inline void min_level(int8_t& min) { min = 1; }
	inline void min_level(int16_t& min) { min = 1; }
	inline void min_level(int32_t& min) { min = 1; }
	inline void min_level(float& min) { min = FLT_MIN; }
	inline void min_level(double& min) { min = DBL_MIN; }

	inline void max_level(int8_t& min) { min = 0x7f; }
	inline void max_level(int16_t& min) { min = 0x7fff; }
	inline void max_level(int32_t& min) { min = 0x7fffffff; }
	inline void max_level(float& max) { max = FLT_MAX; }
	inline void max_level(double& max) { max = DBL_MAX; }

	template <typename T>
	T min_value() {
		T min;
		min_level(min);
		return min;
	}

	template <typename T>
	T max_value() {
		T max;
		max_level(max);
		return max;
	}

	template <typename T> struct vertex2;
	template <typename T> struct vertex3;
	template <typename T> struct vertex4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	8 ビット整数、二次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex2<int8_t>	bpos;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	16 ビット整数、二次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex2<int16_t> spos;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット整数、二次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex2<int32_t> ipos;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット浮動小数点、二次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex2<float>	fpos;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	64 ビット倍精度浮動小数点、二次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex2<double>	dpos;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	8 ビット整数、三次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex3<int8_t> bvtx;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	16 ビット整数、三次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex3<int16_t> svtx;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット整数、三次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex3<int32_t> ivtx;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット浮動小数点、三次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex3<float> fvtx;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	64 ビット倍精度浮動小数点、三次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex3<double> dvtx;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	8 ビット整数、四次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex4<int8_t> bvtx4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	16 ビット整数、四次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex4<int16_t> svtx4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット整数、四次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex4<int32_t> ivtx4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	32 ビット浮動小数点、四次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex4<float> fvtx4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	64 ビット倍精度浮動小数点、四次元、位置情報
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	typedef vertex4<double> dvtx4;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	次元テンプレート共通クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T, int NUM>
	struct vertex_base {
		typedef T   value_type;

		inline uint32_t dim() const { return NUM; }
		inline uint32_t size() const { return sizeof(T) * NUM; }
		static inline T get_min() { T v; min_level(v); return v; }
		static inline T get_max() { T v; max_level(v); return v; }
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	二次元テンプレート
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	struct vertex2 : public vertex_base<T, 2> {
		T	x;
		T	y;

		vertex2() { }
		constexpr vertex2(const spos& v) : x(v.x), y(v.y) { }
		constexpr vertex2(const ipos& v) : x(v.x), y(v.y) { }
		constexpr vertex2(const fpos& v) : x(v.x), y(v.y) { }
		constexpr vertex2(const dpos& v) : x(v.x), y(v.y) { }
		explicit constexpr vertex2(T c) : x(c), y(c) { }
		constexpr vertex2(T xx, T yy) : x(xx), y(yy) { }

		const T* getXY() const { return &x; }

		T sqrX() const { return x * x; }
		T sqrY() const { return y * y; }
		T sqr() const { return sqrX() + sqrY(); }
		T len() const { return fsqrt(sqr()); }

		T min() const { return x < y ? x : y; }
		T max() const { return x > y ? x : y; }

		void set(T c) { x = y = c; }
		void set(T xx, T yy) { x = xx; y = yy; }
		void set(const vertex2& v) { x = v.x; y = v.y; }

		void add(T c) { x += c; y += c; }
		void add(T xx, T yy) { x += xx; y += yy; }
		void add(const vertex2<T>& v) { x += v.x; y += v.y; }
		void sub(T c) { x -= c; y -= c; }
		void sub(T xx, T yy) { x -= xx; y -= yy; }
		void sub(const vertex2<T>& v) { x -= v.x; y -= v.y; }
		void mul(T c) { x *= c; y *= c; }
		void mul(T xx, T yy) { x *= xx; y *= yy; }
		void mul(const vertex2<T>& v) { x *= v.x; y *= v.y; }
		void div(T c) { x /= c; y /= c; }
		void div(T xx, T yy) { x /= xx; y /= yy; }
		void div(const vertex2<T>& v) { x /= v.x; y /= v.y; }

		static T dot(const vertex2& a, const vertex2& b) { return a.x * b.x + a.y * b.y; }
		static T cross(const vertex2& a, const vertex2& b) { return a.x * b.y - a.y * b.x; }

		// Method necessary for using「boost/unordered_map」
		bool operator == (const vertex2<T>& v) const {
			if(x == v.x && y == v.y) return true;
			else return false;
		}

		bool operator != (const vertex2<T>& v) const {
			if(x == v.x && y == v.y) return false;
			else return true;
		}

		size_t hash() const {
			size_t h = 0;
			boost::hash_combine(h, x);
			boost::hash_combine(h, y);
			return h;
		}

		vertex2& operator = (T c) { set(c); return *this; }

		vertex2& operator = (const spos& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y)); return *this;
		}
		vertex2& operator = (const ipos& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y)); return *this;
		}
		vertex2& operator = (const fpos& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y)); return *this;
		}
		vertex2& operator = (const dpos& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y)); return *this;
		}

		vertex2& operator += (const vertex2& v) { add(v); return *this; }
		vertex2& operator += (T c) { add(c); return *this; }
		vertex2 operator + (T c) const {
			vertex2 t(x + c, y + c);
			return t;
		}
		vertex2 operator + (const vertex2& v) const {
			vertex2	t(x + v.x, y + v.y);
			return t;
		}

		vertex2& operator -= (const vertex2& v) { sub(v); return *this; }
		vertex2& operator -= (T c) { sub(c); return *this; }
		vertex2 operator - (T c) const {
			vertex2	t(x - c, y - c);
			return t;
		}
		vertex2 operator - (const vertex2& v) const {
			vertex2 t(x, y);
			t.sub(v);
			return t;
		}

		vertex2& operator *= (T c) { mul(c); return *this; }
		vertex2& operator *= (const vertex2& v) { mul(v);  return *this; }
		vertex2 operator * (const vertex2& v) const {
			vertex2	t(x * v.x, y * v.y);
			return t;
		}
		vertex2 operator * (T c) const {
			vertex2	t(x * c, y * c);
			return t;
		}

		vertex2& operator /= (T c) { div(c); return *this; }
		vertex2& operator /= (const vertex2& v) { div(v); return *this; }
		vertex2 operator / (T c) const {
			vertex2	t(x / c, y / c);
			return t;
		}
		vertex2 operator / (const vertex2& v) const {
			vertex2	t(x / v.x, y / v.y);
			return t;
		}
	};

	typedef std::vector<bpos>					bposs;
	typedef std::vector<bpos>::iterator			bposs_it;
	typedef std::vector<bpos>::const_iterator	bposs_cit;

	typedef std::vector<spos>					sposs;
	typedef std::vector<spos>::iterator			sposs_it;
	typedef std::vector<spos>::const_iterator	sposs_cit;

	typedef std::vector<ipos>					iposs;
	typedef std::vector<ipos>::iterator			iposs_it;
	typedef std::vector<ipos>::const_iterator	iposs_cit;

	typedef std::vector<fpos>					fposs;
	typedef std::vector<fpos>::iterator			fposs_it;
	typedef std::vector<fpos>::const_iterator	fposs_cit;

	typedef std::vector<dpos>					dposs;
	typedef std::vector<dpos>::iterator			dposs_it;
	typedef std::vector<dpos>::const_iterator	dposs_cit;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	三次元、位置情報テンプレート
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	struct vertex3 : public vertex_base<T, 3> {
		T	x;
		T	y;
		T	z;

		inline vertex3() { }
		inline vertex3(const svtx& v) : x(v.x), y(v.y), z(v.z) { }
		inline vertex3(const ivtx& v) : x(v.x), y(v.y), z(v.z) { }
		inline vertex3(const fvtx& v) : x(v.x), y(v.y), z(v.z) { }
		inline vertex3(const dvtx& v) : x(v.x), y(v.y), z(v.z) { }
		explicit inline vertex3(T c) : x(c), y(c), z(c) { }
		explicit inline vertex3(T xx, T yy, T zz = static_cast<T>(0)) : x(xx), y(yy), z(zz) { }

		inline const T* getXYZ() const { return &x; }

		inline T sqrX() const { return x * x; }
		inline T sqrY() const { return y * y; }
		inline T sqrZ() const { return z * z; }
		inline T sqr() const { return sqrX() + sqrY() + sqrZ(); }
		inline T len() const { return fsqrt(sqr()); }

		inline T min() const {
			if(x < y) {
				if(x < z) return x;
			} else {
				if(y < z) return y;
			}
			return z;
		}
		inline T max() const {
			if(x > y) {
				if(x > z) return x;
			} else {
				if(y > z) return y;
			}
			return z;
		}

		inline void set(T c) { x = y = z = c; }
		inline void set(T xx, T yy, T zz = static_cast<T>(0)) { x = xx; y = yy; z = zz; }
		inline void set(const vertex2<T>& v) { x = v.x; y = v.y; z = static_cast<T>(0); }
		inline void set(const vertex3<T>& v) { x = v.x; y = v.y; z = v.z; }

		inline void add(T c) { x += c; y += c; z += c; }
		inline void add(T xx, T yy, T zz) { x += xx; y += yy; z += zz; }
		inline void add(const vertex3& v) { x += v.x; y += v.y; z += v.z; }
		inline void sub(T c) { x -= c; y -= c; z -= c; }
		inline void sub(T xx, T yy, T zz) { x -= xx; y -= yy; z -= zz; }
		inline void sub(const vertex3& v) { x -= v.x; y -= v.y; z -= v.z; }
		inline void mul(T c) { x *= c; y *= c; z *= c; }
		inline void mul(T xx, T yy, T zz) { x *= xx; y *= yy; z *= zz; }
		inline void mul(const vertex3& v) { x *= v.x; y *= v.y; z *= v.z; }
		inline void div(T c) { x /= c; y /= c; z /= c; }
		inline void div(T xx, T yy, T zz) { x /= xx; y /= yy; z /= zz; }
		inline void div(const vertex3& v) { x /= v.x; y /= v.y; z /= v.z; }

		static inline T dot(const vertex3& a, const vertex3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
		static inline void cross(const vertex3& a, const vertex3& b, vertex3& out) {
			out.x = a.y * b.z - a.z * b.y;
			out.y = a.z * b.x - a.x * b.z;
			out.z = a.x * b.y - a.y * b.x;
		}

		// Method necessary for using「boost/unordered_map」
		inline bool operator == (const vertex3& v) const {
			if(x == v.x && y == v.y && z == v.z) return true;
			else return false;
		}

		inline bool operator != (const vertex3& v) const {
			if(x == v.x && y == v.y && z == v.z) return false;
			else return true;
		}

		inline size_t hash() const {
			size_t h = 0;
			boost::hash_combine(h, x);
			boost::hash_combine(h, y);
			boost::hash_combine(h, z);
			return h;
		}

		inline vertex3& operator = (T c) { set(c); return *this; }
		inline vertex3& operator = (const vertex2<T>& v) { set(v); return *this; }

		vertex3& operator = (const svtx& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y), static_cast<T>(v.z));
			return *this;
		}
		vertex3& operator = (const ivtx& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y), static_cast<T>(v.z));
			return *this;
		}
		vertex3& operator = (const fvtx& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y), static_cast<T>(v.z));
			return *this;
		}
		vertex3& operator = (const dvtx& v) {
			set(static_cast<T>(v.x), static_cast<T>(v.y), static_cast<T>(v.z));
			return *this;
		}

		inline vertex3& operator += (T c) { add(c); return *this; }
		inline vertex3& operator += (const vertex3& v) { add(v); return *this; }
		inline vertex3 operator + (T c) const {
			vertex3	t(x + c, y + c, z + c);
			return t;
		}
		inline vertex3 operator + (const vertex3& v) const {
			vertex3	t(x + v.x, y + v.y, z + v.z);
			return t;
		}

		inline vertex3& operator -= (T c) { sub(c); return *this; }
		inline vertex3& operator -= (const vertex3& v) { sub(v); return *this; }
		inline vertex3 operator - (T c) const {
			vertex3	t(x - c, y - c, z - c);
			return t;
		}
		inline vertex3 operator - (const vertex3& v) const {
			vertex3	t(x - v.x, y - v.y, z - v.z);
			return t;
		}

		inline vertex3& operator *= (T c) { mul(c); return *this; }
		inline vertex3& operator *= (const vertex3& v) { mul(v); return *this; }
		inline vertex3 operator * (T c) const {
			vertex3	t(x * c, y * c, z * c);
			return t;
		}
		inline vertex3 operator * (const vertex3& v) const {
			vertex3	t(x * v.x, y * v.y, z * v.z);
			return t;
		}

		inline vertex3& operator /= (T c) { div(c); return *this; }
		inline vertex3& operator /= (const vertex3& v) { div(v); return *this; }
		inline vertex3 operator / (T c) const {
			vertex3	t(x / c, y / c, z / c);
			return t;
		}
		inline vertex3 operator / (const vertex3& v) const {
			vertex3	t(x / v.x, y / v.y, z / v.z);
			return t;
		}

	};

	typedef std::vector<bvtx>					bvtxs;
	typedef std::vector<bvtx>::iterator			bvtxs_it;
	typedef std::vector<bvtx>::const_iterator	bvtxs_cit;

	typedef std::vector<svtx>					svtxs;
	typedef std::vector<svtx>::iterator			svtxs_it;
	typedef std::vector<svtx>::const_iterator	svtxs_cit;

	typedef std::vector<ivtx>					ivtxs;
	typedef std::vector<ivtx>::iterator			ivtxs_it;
	typedef std::vector<ivtx>::const_iterator	ivtxs_cit;

	typedef std::vector<fvtx>					fvtxs;
	typedef std::vector<fvtx>::iterator			fvtxs_it;
	typedef std::vector<fvtx>::const_iterator	fvtxs_cit;

	typedef std::vector<dvtx>					dvtxs;
	typedef std::vector<dvtx>::iterator			dvtxs_it;
	typedef std::vector<dvtx>::const_iterator	dvtxs_cit;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	四次元、位置情報テンプレート
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	struct vertex4 : public vertex_base<T, 4> {
		T	x;
		T	y;
		T	z;
		T	w;

		inline vertex4() { }
		explicit inline vertex4(T c) : x(c), y(c), z(c), w(c) { }
		explicit inline vertex4(T xx, T yy, T zz, T ww = static_cast<T>(1)) : x(xx), y(yy), z(zz), w(ww) { }
		inline vertex4(const vertex3<T>& v) : x(v.x), y(v.y), z(v.z), w(static_cast<T>(1)) { }
		inline vertex4(const vertex4& v) : x(v.x), y(v.y), z(v.z), w(v.w) { }

		inline const T* getXYZW() const { return &x; }

		inline T sqrX() const { return x * x; }
		inline T sqrY() const { return y * y; }
		inline T sqrZ() const { return z * z; }
		inline T sqrW() const { return w * w; }
		inline T sqr() const { return (sqrX() + sqrY() + sqrZ() + sqrW()); }
		inline T len() const { return fsqrt(sqrX() + sqrY() + sqrZ() + sqrW()); }

		inline T min() const {
			if(x < y) {
				if(x < z) {
					if(x < w) return x;
				} else {
					if(z < w) return z;
				}
			} else {
				if(y < z) {
					if(y < w) return y;
				} else {
					if(z < w) return z;
				}
			}
			return w;
		}
		inline T max() const {
			if(x > y) {
				if(x > z) {
					if(x > w) return x;
				} else {
					if(z > w) return z;
				}
			} else {
				if(y > z) {
					if(y > w) return y;
				} else {
					if(z > w) return z;
				}
			}
			return w;
		}

		inline void set(T c) { x = y = z = w = c; }
		inline void set(T xx, T yy, T zz, T ww = static_cast<T>(1)) { x = xx; y = yy; z = zz; w = ww; }
		inline void set(const vertex2<T>& v) { x = v.x; y = v.y; z = static_cast<T>(0); w = static_cast<T>(1); }
		inline void set(const vertex3<T>& v) { x = v.x; y = v.y; z = v.z; w = static_cast<T>(1); }
		inline void set(const vertex4<T>& v) { x = v.x; y = v.y; z = v.z; w = v.w;}

		inline void add(T xx, T yy, T zz, T ww = static_cast<T>(1)) { x += xx; y += yy; z += zz; w += ww; }
		inline void sub(T xx, T yy, T zz, T ww = static_cast<T>(1)) { x -= xx; y -= yy; z -= zz; w -= ww; }
		inline void mul(T xx, T yy, T zz, T ww = static_cast<T>(1)) { x *= xx; y *= yy; z *= zz; w *= ww; }
		inline void div(T xx, T yy, T zz, T ww = static_cast<T>(1)) { x /= xx; y /= yy; z /= zz; w /= ww; }

		inline bool operator == (const vertex4& v) const {
			if(x == v.x && y == v.y && z == v.z && w == v.w) return true;
			else return false;
		}

		inline bool operator != (const vertex4& v) const {
			if(x == v.x && y == v.y && z == v.z && w == v.w) return false;
			else return true;
		}

		// Method necessary for using「boost/unordered_map」
		inline size_t hash() const {
			size_t h = 0;
			boost::hash_combine(h, x);
			boost::hash_combine(h, y);
			boost::hash_combine(h, z);
			boost::hash_combine(h, w);
			return h;
		}

		inline vertex4& operator = (T c) { set(c); return *this; }
		inline vertex4& operator = (const vertex2<T>& v) { set(v); return *this; }
		inline vertex4& operator = (const vertex3<T>& v) { set(v); return *this; }
		inline vertex4& operator = (const vertex4<T>& v) { set(v); return *this; }

		inline vertex4& operator += (T c) { add(c); return *this; }
		inline vertex4& operator += (const vertex4& v) { add(v); return *this; }
		inline vertex4 operator + (T c) const {
			vertex4	t(x + c, y + c, z + c, w + c);
			return t;
		}
		inline vertex4 operator + (const vertex4& v) const {
			vertex4 t(x + v.x, y + v.y, z + v.z, w + v.w);
			return t;
		}

		inline vertex4& operator -= (T c) { sub(c); return *this; }
		inline vertex4& operator -= (const vertex4& v) { sub(v); return *this; }
		inline vertex4 operator - (T c) const {
			vertex4 t(x - c, y - c, z - c, w - c);
			return t;
		}
		inline vertex4<T> operator - (const vertex4<T>& v) const {
			vertex4<T> t(x - v.x, y - v.y, z - v.z, w - v.w);
			return t;
		}

		inline vertex4<T>& operator *= (T c) { mul(c); return *this; }
		inline vertex4<T>& operator *= (const vertex4& v) { mul(v); return *this; }
		inline vertex4<T> operator * (T c) const {
			vertex4<T> t(x * c, y * c, z * c, w * c);
			return t;
		}
		inline vertex4<T> operator * (const vertex4<T>& v) const {
			vertex4<T> t(x * v.x, y * v.y, z * v.z, w * v.w);
			return t;
		}

		inline vertex4<T>& operator /= (T c) { div(c); return *this; }
		inline vertex4<T>& operator /= (const vertex4<T>& v) { div(v); return *this; }
		inline vertex4<T> operator / (T c) const {
			vertex4<T> t(x / c, y / c, z / c, w / c);
			return t;
		}
		inline vertex4<T> operator / (const vertex4<T>& v) const {
			vertex4<T> t(x / v.x, y / v.y, z / v.z, w / v.w);
			return t;
		}
	};

	typedef std::vector<bvtx4>					bvtxs4;
	typedef std::vector<bvtx4>::iterator		bvtxs4_it;
	typedef std::vector<bvtx4>::const_iterator	bvtxs4_cit;

	typedef std::vector<svtx4>					svtxs4;
	typedef std::vector<svtx4>::iterator		svtxs4_it;
	typedef std::vector<svtx4>::const_iterator	svtxs4_cit;

	typedef std::vector<ivtx4>					ivtxs4;
	typedef std::vector<ivtx4>::iterator		ivtxs4_it;
	typedef std::vector<ivtx4>::const_iterator	ivtxs4_cit;

	typedef std::vector<fvtx4>					fvtxs4;
	typedef std::vector<fvtx4>::iterator		fvtxs4_it;
	typedef std::vector<fvtx4>::const_iterator	fvtxs4_cit;

	typedef std::vector<dvtx4>					dvtxs4;
	typedef std::vector<dvtx4>::iterator		dvtxs4_it;
	typedef std::vector<dvtx4>::const_iterator	dvtxs4_cit;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	「boost/unordered_set, unordered_map」用ハッシュ値計算
		@param[in]	v	ハッシュ・ソース座標
		@return		ハッシュ値を返す
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline size_t hash_value(const T& v) { return v.hash(); }

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ベクトルの距離を求める
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@return	ベクトルの距離
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline typename T::value_type distance(const T& a, const T& b) {
		T d = b - a;
		return d.len();
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ベクトルの内積
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@return	内積結果
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline typename T::value_type dot(const T& a, const T& b)
	{
		return T::dot(a, b);
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	二次元ベクトルの外積
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@return 外積結果（スカラー）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline typename T::value_type cross(const T& a, const T& b)
	{
		return T::cross(a, b);
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	三次元ベクトルの外積
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@param[out]	out	外積結果を受け取るベクトル（正規化されていない）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline void cross(const T& a, const T& b, T& out)
	{
		T::cross(a, b, out);
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ベクトルの内積、正規化
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@return	内積結果（COS θ)
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	inline typename T::value_type inner_product(const T& a, const T& b)
	{
		typename T::value_type c = a.len() * b.len();
		if(c <= T::get_min()) return T::get_max();
		return T::dot(a, b) / c;
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ベクトルの外積、正規化
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@param[out]	n	外積結果を受け取るベクトル
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	void outer_product(const T& a, const T& b, T& n)
	{
		T t;
		T::cross(a, b, t);
		typename T::value_type l = t.len();
		if(l <= T::get_min()) {
			n = static_cast<T>(0);
		} else {
			n = t / l;
		}
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	二次元ベクトルの回転(CW: 時計廻り)@n
				X' =  X COS(s) + Y SIN(s)@n
				Y' =  Y COS(s) - X SIN(s)
		@param[in]	src	ソース・ベクトル
		@param[in]	si	サイン
		@param[in]	co	コサイン
		@param[in]	dst	出力・ベクトル
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	void rotate_cw(const T& src, typename T::value_type si, typename T::value_type co, T& dst)
	{
		dst.set(src.x * co + src.y * si, src.y * co - src.x * si);
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	二次元ベクトルの回転(CCW: 反時計廻り)@n
				X' =  X COS(s) - Y SIN(s)@n
				Y' =  Y COS(s) + X SIN(s)
		@param[in]	src	ソース・ベクトル
		@param[in]	si	サイン
		@param[in]	co	コサイン
		@param[in]	dst	出力・ベクトル
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <class T>
	void rotate_ccw(const T& src, typename T::value_type si, typename T::value_type co, T& dst)
	{
		dst.set(src.x * co - src.y * si, src.y * co + src.x * si);
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ライン（対の値を扱う）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	class line {
	public:
		T	s;
		T	t;

		line() { }
		line(const T& ss, const T& tt) { s = ss; t = tt; }
		void set(const T& ss, const T& tt) { s = ss; t = tt; }
		void first(const T& ss) { s = ss; }
		void second(const T& tt) { t = tt; }
		T center() const { return (s + t) / static_cast<T>(2); }
		static T center(const T& ss, const T& tt) { return (ss + tt) / static_cast<T>(2); }
	};

	typedef line<fpos>	fpos_line;
	typedef line<dpos>	dpos_line;

	typedef line<fvtx>	fvtx_line;
	typedef line<dvtx>	dvtx_line;

	typedef std::vector<fpos_line>				fpos_lines;
	typedef std::vector<fpos_line>::iterator	fpos_lines_it;
	typedef std::vector<dpos_line>				dpos_lines;
	typedef std::vector<dpos_line>::iterator	dpos_lines_it;

	typedef std::vector<fvtx_line>				fvtx_lines;
	typedef std::vector<fvtx_line>::iterator	fvtx_lines_it;
	typedef std::vector<dvtx_line>				dvtx_lines;
	typedef std::vector<dvtx_line>::iterator	dvtx_lines_it;

	//-----------------------------------------------------------------//
	/*!
		@brief	vertex を正規化するテンプレート
		@param[in]	src	ソース座標列
		@param[out]	dst	生成座標列
		@return	失敗した場合に「false」を返す
	*/
	//-----------------------------------------------------------------//
	template <class T>
	bool normalize(const T& src, T& dst) {
		typename T::value_type a = src.len();
		if(a < src.get_min()) return false;
		dst = src / a;
		return true;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	vertex を正規化するテンプレート
		@param[in]	src	ソース座標列
		@return	正規化されたベクトル
	*/
	//-----------------------------------------------------------------//
	template <class T>
	T normalize(const T& src) {
		T dst;
		if(normalize(src, dst)) {
			return dst;
		} else {
			return src;
		}
	}

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	ortho_normalize
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@param[out]	dst	結果を受け取るベクトル
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
    template <typename T>
    inline void ortho_normalize(const vertex3<T>& a, const vertex3<T>& b, vertex3<T>& dst)
	{
		normalize(a - b * dot(b, a), dst);
    }

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	二次元領域テンプレート・クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	class rectangle {
	public:
		typedef T	value_type;

		vertex2<T>		org;	///< 開始点
		vertex2<T>		size;	///< サイズ

		rectangle() { }
		constexpr rectangle(T v) : org(v), size(v) { }
		constexpr rectangle(T x, T y, T w, T h) : org(x, y), size(w, h) { }
		constexpr rectangle(const vertex2<T>& org_, const vertex2<T>& size_) : org(org_), size(size_) { }
		constexpr rectangle(const rectangle<T>& r) : org(r.org), size(r.size) { }

		void set(T x, T y, T w, T h) { org.set(x, y); size.set(w, h); }
		void set(const vertex2<T>& org_, const vertex2<T>& size_) { org = org_; size = size_; }

		bool is_focus(const vertex2<T>& p) const {
			if(size.x <= 0 || size.y <= 0) return false;
			if(org.x <= p.x && p.x < (org.x + size.x) && org.y <= p.y && p.y < (org.y + size.y)) {
				return true;
			} else {
				return false;
			}
		}

		void center(vertex2<T>& c) const { c = (org + size) / static_cast<T>(2); }
		vertex2<T> center() const { return (org + size) / static_cast<T>(2); }
		void end(vertex2<T>& e) const { e = org + size; }
		vertex2<T> end() const { return org + size; }

		T center_x() const { return org.x + size.x / 2; }
		T center_y() const { return org.y + size.y / 2; }
		T end_x() const { return org.x + size.x; }
		T end_y() const { return org.y + size.y; }

		// クリップ領域の構築
		bool clip(const rectangle& r) {
			vertex2<T> e = end();
			vertex2<T> re = r.end();
			if(r.org.x <= org.x && org.x < re.x) {
				if(re.x < e.x) {
					size.x = re.x - org.x;
				}
			} else if(re.x <= org.x) {
				size.x = 0;
				return false;
			} else if(org.x < r.org.x) {
				if(e.x < r.org.x) {
					size.x = 0;
					return false;
				} else {
					org.x = r.org.x;
					if(re.x < e.x) {
						size.x = re.x - org.x;
					}
				}
			}

			if(r.org.y <= org.y && org.y < re.y) {
				if(re.y < e.y) {
					size.y = re.y - org.y;
				}
			} else if(re.y <= org.y) {
				size.y = 0;
				return false;
			} else if(org.y < r.org.y) {
				if(e.y < r.org.y) {
					size.y = 0;
					return false;
				} else {
					org.y = r.org.y;
					if(re.y < e.y) {
						size.y = re.y - org.y;
					}
				}
			}

			return true;
		}
	};

	typedef rectangle<int16_t> srect;
	typedef rectangle<int32_t> irect;
	typedef rectangle<float>   frect;
	typedef rectangle<double>  drect;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	サークル
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	class circle {
		T				radius_;
		T				radius_sqr_;
	public:
		vertex2<T>		center;
		circle() { }
		circle(const vertex2<T>& cen, T r) : center(cen), radius_(r), radius_sqr_(r * r) { }
		circle(T x, T y, T r) : center(x, y), radius_(r), radius_sqr_(r * r) { }

		T get_radius() const { return radius_; }
		void set_radius(T r) { radius_ = r; radius_sqr_ = r * r; }

		bool is_focus(const vertex2<T>& p) {
			vertex2<T> d = center - p;
			if((d.x * d.x + d.y * d.y) <= radius_sqr_) return true;
			else return false;
		}
	};

	typedef circle<int>		icircle;
	typedef circle<float>	fcircle;
	typedef circle<double>	dcircle;

	//-----------------------------------------------------------------//
	/*!
		@brief	クランプした B-Spline の生成
		@param[in]	src	ソース座標列
		@param[in]	lod	分割数
		@param[out]	dst	生成座標列
		@return	失敗した場合に「false」を返す
	*/
	//-----------------------------------------------------------------//
	bool clumped_bspline(const fvtxs& src, int lod, fvtxs& dst);

	//-----------------------------------------------------------------//
	/*!
		@brief	分割したベクトル列の生成
		@param[in]	src	ソース座標列
		@param[in]	lod	分割数
		@param[out]	dst	生成座標列
		@return	失敗した場合に「false」を返す
	*/
	//-----------------------------------------------------------------//
	bool division_lines(const fvtxs& src, int lod, fvtxs& dst);

	//-----------------------------------------------------------------//
	/*!
		@brief	直線の交点を求める(float)
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@param[out]	pos	交点
		@return	「解」が無い場合、「false」が返る。
	*/
	//-----------------------------------------------------------------//
	bool intersection_line(const fpos_line& a, const fpos_line& b, fpos& pos);

	//-----------------------------------------------------------------//
	/*!
		@brief	直線の交点を求める(double)
		@param[in]	a	ベクトル A
		@param[in]	b	ベクトル B
		@param[out]	pos	交点
		@return	「解」が無い場合、「false」が返る。
	*/
	//-----------------------------------------------------------------//
	bool intersection_line(const dpos_line& a, const dpos_line& b, dpos& pos);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値を設定
		@param[in]	src		ソース
		@param[out]	min		最小値
	*/
	//-----------------------------------------------------------------//
	void set_min(const fvtx& src, fvtx& min);

	//-----------------------------------------------------------------//
	/*!
		@brief	最大値を設定
		@param[in]	src		ソース
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void set_max(const fvtx& src, fvtx& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値と最大値を設定
		@param[in]	src		ソース
		@param[out]	min		最小値
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void set_min_max(const fvtx& src, fvtx& min, fvtx& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値と最大値を探す (fpos)
		@param[in]	src		ソース列
		@param[out]	min		最小値
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void scan_min_max(const fposs& src, fpos& min, fpos& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値と最大値を探す (dpos)
		@param[in]	src		ソース列
		@param[out]	min		最小値
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void scan_min_max(const dposs& src, dpos& min, dpos& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値と最大値を探す (fvtx)
		@param[in]	src		ソース列
		@param[out]	min		最小値
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void scan_min_max(const fvtxs& src, fvtx& min, fvtx& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	最小値と最大値を探す (dvtx)
		@param[in]	src		ソース列
		@param[out]	min		最小値
		@param[out]	max		最大値
	*/
	//-----------------------------------------------------------------//
	void scan_min_max(const dvtxs& src, dvtx& min, dvtx& max);

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトルの中心位置（fpos)
		@param[in]	a	a 点
		@param[in]	b	b 点
		@param[out]	cen	中心位置
	*/
	//-----------------------------------------------------------------//
	inline void center_line(const fpos& a, const fpos& b, fpos& out) {
		out = fpos_line::center(a, b);
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトルの中心位置（dpos)
		@param[in]	a	a 点
		@param[in]	b	b 点
		@param[out]	cen	中心位置
	*/
	//-----------------------------------------------------------------//
	inline void center_line(const dpos& a, const dpos& b, dpos& out) {
		out = dpos_line::center(a, b);
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトルの中心位置（fvtx)
		@param[in]	a	a 点
		@param[in]	b	b 点
		@param[out]	cen	中心位置
	*/
	//-----------------------------------------------------------------//
	inline void center_line(const fvtx& a, const fvtx& b, fvtx& out) {
		out = fvtx_line::center(a, b);
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトルの中心位置（dvtx)
		@param[in]	a	a 点
		@param[in]	b	b 点
		@param[out]	cen	中心位置
	*/
	//-----------------------------------------------------------------//
	inline void center_line(const dvtx& a, const dvtx& b, dvtx& out) {
		out = dvtx_line::center(a, b);
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクター比（fvtx)
		@param[in]	a		a 点
		@param[in]	factor	比率
		@param[in]	b		b 点
		@param[out]	out		答え
	*/
	//-----------------------------------------------------------------//
	inline void factor_line(const fvtx& a, float factor, const fvtx& b, fvtx& out) {
		out = a + (b - a) * factor;
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトル列の平均（fpos)
		@param[in]	list	ベクトル列
		@param[out]	out		平均値
	*/
	//-----------------------------------------------------------------//
	inline void average(const fposs& list, fpos& out) {
		out.set(0.0f, 0.0f);
		for(fposs_cit cit = list.begin(); cit != list.end(); ++cit) {
			out += *cit;
		}
		out *= 1.0f / static_cast<float>(list.size());
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	ベクトル列の平均（fvtx)
		@param[in]	list	ベクトル列
		@param[out]	out		平均値
	*/
	//-----------------------------------------------------------------//
	inline void average(const fvtxs& list, fvtx& out) {
		out.set(0.0f, 0.0f, 0.0f);
		for(fvtxs_cit cit = list.begin(); cit != list.end(); ++cit) {
			out += *cit;
		}
		out *= 1.0f / static_cast<float>(list.size());
	}

	//-----------------------------------------------------------------//
	/*!
		@brief	二つの頂点配列(std::vector)から、共有「する」、@n
				「しない」頂点配列(std::vector)を生成
		@param[in]	src_a	配列 A
		@param[in]	src_b	配列 B
		@param[out]	dst		共有配列
		@param[in]	share	「false」なら共有しない頂点を生成
		@return 共有する頂点が無い場合「false」
	*/
	//-----------------------------------------------------------------//
	bool make_share_vertex(const fvtxs& src_a, const fvtxs& src_b, fvtxs& dst, bool share = true);

	typedef boost::unordered_set<fvtx>					fvtx_set;
	typedef boost::unordered_set<fvtx>::iterator		fvtx_set_it;
	typedef boost::unordered_set<fvtx>::const_iterator	fvtx_set_cit;

	typedef boost::unordered_set<dvtx>					dvtx_set;
	typedef boost::unordered_set<dvtx>::iterator		dvtx_set_it;
	typedef boost::unordered_set<dvtx>::const_iterator	dvtx_set_cit;

}	// namespace vtx


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include <debug.h>

#include "Bullet2d3DR.hpp"
#include <RigidBody2dDS.hpp>
#include <Interaction.hpp>

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunreachable-code"
#pragma clang diagnostic ignored "-Woverloaded-virtual"
#elif !(__INTEL_COMPILER || __APPLE__ )
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Woverloaded-virtual"
#endif

#include <BulletCollision/NarrowPhaseCollision/btManifoldPoint.h>
#include <BulletCollision/CollisionDispatch/btCollisionObject.h>

#include <btBulletCollisionCommon.h>

#if defined(__clang__)
#pragma clang diagnostic pop
#elif !(__INTEL_COMPILER || __APPLE__ )
#pragma GCC diagnostic pop
#endif

#include <boost/math/quaternion.hpp>

//static
//void copyQuatRot2d(boost::math::quaternion<double>& from, SiconosVector& to)
//{
//  double angle = 2.0 * acos(from.R_component_1());
//  to(2) = angle;
//}

static
void copyQuatRot2d(const SiconosVector& from, boost::math::quaternion<double>& to)
{
  double half_angle = from(2)/2.0;

  to = boost::math::quaternion<double>(cos(half_angle), 0.0, 0.0, sin(half_angle));
}

static
void copyQuatPos2d(const boost::math::quaternion<double>& from, SiconosVector& to)
{
  to(0) = from.R_component_2();
  to(1) = from.R_component_3();
}

static
void copyQuatPos2d(const SiconosVector& from, boost::math::quaternion<double>& to)
{
  to = boost::math::quaternion<double>(0, from(0), from(1), 0.0);
}

static
void copyQuatPos2d(const btVector3& from, boost::math::quaternion<double>& to)
{
  to = boost::math::quaternion<double>(0, from.x(), from.y(), 0.0);
}

static void copyBtVector32d(const btVector3 &from, SiconosVector& to)
{
  to(0) = from.x();
  to(1) = from.y();
}

Bullet2d3DR::Bullet2d3DR()
  : Contact2d3DR()
{
}

#ifdef DEBUG_MESSAGES
static
void display_quat(boost::math::quaternion<double>& quat)
{
  std::cout << "q_0: " << quat.R_component_1()
            << " q_1: " << quat.R_component_2()
            << " q_2: " << quat.R_component_3()
            << " q_3: " << quat.R_component_4() << std::endl;
}
#endif

void Bullet2d3DR::updateContactPointsFromManifoldPoint(const btPersistentManifold& manifold,
    const btManifoldPoint& point,
    bool flip, double scaling,
    SP::RigidBody2dDS ds1,
    SP::RigidBody2dDS ds2)
{
  DEBUG_BEGIN("Bullet2d3DR::updateContactPointsFromManifoldPoint(...)\n");
  // Get new world positions of contact points and calculate relative
  // to ds1 and ds2

  DEBUG_PRINTF("point.getPositionWorldOnA().x() = %8.5e\t", point.getPositionWorldOnA().x());
  DEBUG_PRINTF("point.getPositionWorldOnA().y() = %8.5e\t", point.getPositionWorldOnA().y());
  DEBUG_PRINTF("point.getPositionWorldOnA().z() = %8.5e\n", point.getPositionWorldOnA().z());

  DEBUG_PRINTF("point.getPositionWorldOnB().x() = %8.5e\t", point.getPositionWorldOnB().x());
  DEBUG_PRINTF("point.getPositionWorldOnB().y() = %8.5e\t", point.getPositionWorldOnB().y());
  DEBUG_PRINTF("point.getPositionWorldOnB().z() = %8.5e\n", point.getPositionWorldOnB().z());

  DEBUG_PRINTF("point.m_normalWorldOnB.x() = %8.5e\t", point.m_normalWorldOnB.x());
  DEBUG_PRINTF("point.m_normalWorldOnB.y() = %8.5e\t", point.m_normalWorldOnB.y());
  DEBUG_PRINTF("point.m_normalWorldOnB.z() = %8.5e\n", point.m_normalWorldOnB.z());

  ::boost::math::quaternion<double> rq1, rq2, posa;
  ::boost::math::quaternion<double> pq1, pq2, posb;

  /* Compute quaternion representation of the position of ds1 and the rotation */
  DEBUG_EXPR(ds1->q()->display(););
  copyQuatPos2d(*ds1->q(), pq1);

  copyQuatRot2d(*ds1->q(), rq1);
  DEBUG_EXPR(display_quat(pq1););
  DEBUG_EXPR(display_quat(rq1););

  if(ds2)
  {
    DEBUG_EXPR(ds2->q()->display(););
    copyQuatPos2d(*ds2->q(), pq2);
    copyQuatRot2d(*ds2->q(), rq2);
  }

  /* Compute a quaternion representation of the position of the contact points
   * to prepare rotations
   * posa : global position of the contact point A on ds1 if flip =0
   * posb : global position of the contact point B on ds2 if flip =0
   */
  copyQuatPos2d(point.getPositionWorldOnA() / scaling, posa);
  copyQuatPos2d(point.getPositionWorldOnB() / scaling, posb);

  if(flip)
  {
    ::boost::math::quaternion<double> tmp = posa;
    posa = posb;
    posb = tmp;
  }

  /* after flips :
   * posa : global position of the contact point A on ds1
   * posb : global position of the contact point B on ds2
   */
  DEBUG_EXPR(display_quat(posa););
  DEBUG_EXPR(display_quat(posb););

  SiconosVector va(2), vb(2);
  if(flip)
  {
    /* Rotate the relatice position of the contact point */
    copyQuatPos2d((1.0/rq1) * (posa - pq1) * rq1, va);

    if(ds2)
      copyQuatPos2d((1.0/rq2) * (posb - pq2) * rq2, vb);
    else
    {
      // If no body2, position is relative to 0,0,0
      copyBtVector32d(point.getPositionWorldOnA() / scaling, vb);
    }
  }
  else
  {
    copyQuatPos2d((1.0/rq1) * (posa - pq1) * rq1, va);
    if(ds2)
      copyQuatPos2d((1.0/rq2) * (posb - pq2) * rq2, vb);
    else
    {
      // If no body2, position is relative to 0,0,0
      copyBtVector32d(point.getPositionWorldOnB() / scaling, vb);
    }
  }

  SiconosVector vn(3);
  // Get new normal
  if(ds2)
  {
    btQuaternion qn(point.m_normalWorldOnB.x(),
                    point.m_normalWorldOnB.y(),
                    point.m_normalWorldOnB.z(), 0);
    btQuaternion qb1 = manifold.getBody1()->getWorldTransform().getRotation();
    // un-rotate normal into body1 frame
    qn = qb1.inverse() * qn * qb1;
    vn(0) = qn.x();
    vn(1) = qn.y();
    vn(2) = qn.z();
    vn = vn/vn.norm2();
  }
  else
    copyBtVector32d(point.m_normalWorldOnB, vn);

  vn.resize(2);
  DEBUG_EXPR(va.display(););
  DEBUG_EXPR(vb.display(););
  DEBUG_EXPR(vn.display(););
  Contact2d3DR::updateContactPoints(va, vb, vn*(flip?-1:1));
  DEBUG_END("Bullet2d3DR::updateContactPointsFromManifoldPoint(...)\n");
}


// Copyright (C) 2018 Thejaka Amila Kanewala, Marcin Zalewski, Andrew Lumsdaine.

// Boost Software License - Version 1.0 - August 17th, 2003

// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//  Authors: Thejaka Kanewala
//           Marcin Zalewski
//           Andrew Lumsdaine

//======== Maximal Independent Set Algortihms================//
// Driver for MIS family of algorithms.
//===========================================================//

#include <iostream>
#include "common/synthetic_generator.hpp"
#include "common/parser.hpp"
#include "common/executor.hpp"
#include "common/vertex_permutations.hpp"
#include <boost/graph/distributed/mis_delta.hpp>
#include <boost/graph/distributed/mis.hpp>
#include <boost/graph/distributed/luby_mis.hpp>
#include <boost/graph/distributed/thread_pq_def.hpp>

enum mis_algorithm {
  fix,
  fix_bucket,
  luby_a,
  luby_av1,
  luby_av2,
  luby_b
};

class mis_instance_params {

public:
  mis_algorithm algorithm;
  id_distribution_t id_distribution;
  bool verify;
  mis_instance_params(mis_algorithm& alg,
		      id_distribution_t& idd,
		      bool v):
    algorithm(alg), id_distribution(idd), verify(v){}

  void print() {
      std::cout << "Algorithm -- ";
      if (algorithm == fix)
	std::cout << "FIX";
      else if(algorithm == fix_bucket)
	std::cout << "FIX-Bucket";
      else if(algorithm == luby_a)
	std::cout << "Luby A";
      else if(algorithm == luby_av1)
	std::cout << "Luby AV1";
      else if(algorithm == luby_av2)
	std::cout << "Luby AV2";
      else if(algorithm == luby_b)
	std::cout << "Luby B";
      else
	std::cerr << "Invalid !" << std::endl;

      std::cout << std::endl;

    if (id_distribution == vertical)
      std::cout << "id distribution : vertical" << std::endl;

    if (id_distribution == horizontal)
      std::cout << "id distribution : horizontal" << std::endl;

    std::cout << "verify : " << verify << std::endl;
  }

  std::string get_algorithm() {
    if (algorithm == fix)
      return "FIX";
    else if(algorithm == fix_bucket)
      return "FIX-Bucket";
    else if(algorithm == luby_a)
      return "Luby A";
    else if(algorithm == luby_av1)
      return "Luby AV1";
    else if(algorithm == luby_av2)
      return "Luby AV2";
    else if(algorithm == luby_b)
      return "Luby B";
    else
      return "Invalid !";
  }

};

class mis_params {

private:
  std::vector<mis_instance_params> instance_params;
  std::vector<mis_algorithm> algorithms;
  id_distribution_t id_distribution = horizontal; // default
  bool verify = false;

public:
  bool parse(int argc, char* argv[]){
    for (int i = 1; i < argc; ++i) {
      mis_algorithm algorithm;
      std::string arg = argv[i];
      if (arg == "--run_fix_mis") {
	algorithm = fix;
	algorithms.push_back(algorithm);
      }

      if (arg == "--run_bucket_mis") {
	algorithm = fix_bucket;
	algorithms.push_back(algorithm);
      }

      if (arg == "--id-distribution") {
	if (strcmp(argv[i+1],"vertical") == 0)
	  id_distribution = vertical;
	else if (strcmp(argv[i+1],"horizontal") == 0)
	  id_distribution = horizontal;
	else {
	  std::cout << "Invalid id distribution type. Available types are vertical and horizontal"
		    << std::endl;
	  return false;
	}
      }

      if (arg == "--verify") {
	verify = true;
      }

      if (arg == "--luby_algorithms") {
	std::vector<std::string> luby_algorithms;
	luby_algorithms = extract_params<std::string> ( argv[i+1] );
	BOOST_FOREACH(std::string al, luby_algorithms) {
	  if (al == "A")
	    algorithm = luby_a;
	  else if (al == "AV1")
	    algorithm = luby_av1;
	  else if (al == "AV2")
	    algorithm = luby_av2;
	  else if (al == "B")
	    algorithm = luby_b;
	  else {
	    std::cerr << "Invalid Luby algorithm. Available algorithms --"
		      << "A, AV1, AV2, B" << std::endl;
	    return false;
	  }

	  algorithms.push_back(algorithm);
	}
      }
    }
  }

  void print() {
    BOOST_FOREACH(mis_algorithm alg, algorithms) {
      std::cout << "Algorithm -- ";
      if (alg == fix)
	std::cout << "FIX";
      else if(alg == fix_bucket)
	std::cout << "FIX-Bucket";
      else if(alg == luby_a)
	std::cout << "Luby A";
      else if(alg == luby_av1)
	std::cout << "Luby AV1";
      else if(alg == luby_av2)
	std::cout << "Luby AV2";
      else if(alg == luby_b)
	std::cout << "Luby B";
      else
	std::cerr << "Invalid !" << std::endl;

      std::cout << std::endl;
    }

    if (id_distribution == vertical)
      std::cout << "id distribution : vertical" << std::endl;

    if (id_distribution == horizontal)
      std::cout << "id distribution : horizontal" << std::endl;

    std::cout << "verify : " << verify << std::endl;
  }

  const std::vector<mis_instance_params>&
  get_instance_params() {
    if (instance_params.empty()) {
      BOOST_FOREACH(mis_algorithm alg, algorithms) {
	instance_params.push_back(mis_instance_params(alg, id_distribution, verify));
      }
    }

    return instance_params;
  }

};

class MISExecutor {
private:

  template <typename Graph, typename MISMap>
  bool verify_mis(amplusplus::transport& trans,  Graph& g, MISMap& mis) {
    typedef typename boost::property_map<Graph, boost::vertex_owner_t>::const_type OwnerMap;
    OwnerMap owner(get(boost::vertex_owner, g));

    mis.set_consistency_model(boost::parallel::cm_forward || boost::parallel::cm_backward);
    mis.set_max_ghost_cells(0);

    if (trans.rank() == 0) std::cout<<"Verifying MIS results......";

    {
      amplusplus::scoped_epoch epoch(g.transport());

      BGL_FORALL_VERTICES_T(v, g, Graph) {
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	  get(mis, target(e, g));
	}
      }
    }

    bool result = true;
    int found_mis = 0;
#ifdef PRINT_DEBUG
    std::cout << "MIS = {";
#endif
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (get(mis, v) ==  MIS_FIX1) {// v in mis, none of the neigbours should be in mis
#ifdef PRINT_DEBUG
	if (v == 35) {
	  std::cout << "Printing all neighbors of " << v
		    << " -- [";

	  BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	    std::cout << target(e, g) << ", ";
	  }
	  std::cout << "]" << std::endl;
	}

	std::cout << v << "-[";
#endif
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
#ifdef PRINT_DEBUG
	  std::cout << target(e, g) << ", ";
#endif
	  if (v == target(e, g))
	    continue;

	  if (get(mis, target(e, g)) == MIS_FIX1) {
	    std::cout << "[FAIL] Vertex : " << v << " and its neigbour " << target(e, g)
		      << " are both in MIS."
		      << " Vertex value : " << get(mis, v)
		      << " neighbor value : " << get(mis, target(e, g))
		      << std::endl;
	    std::cout << "Neighbors of " << v << "- {";
	    BGL_FORALL_ADJ_T(v, u1, g, Graph) {
	      std::cout << "(" << u1 << ", " << get(mis, u1) << ")";
	    }
	    std::cout << "}" << std::endl;

	    auto k = target(e, g);
	    if (get(owner, k) == g.transport().rank()) {
	      std::cout << "Neighbors of " << k << "- {";
	      BGL_FORALL_ADJ_T(k, u2, g, Graph) {
		std::cout << "(" << u2 << ", " << get(mis, u2) << ")";
	      }
	      std::cout << "}" << std::endl;
	    }

	    result = false;
	  } else {
#ifdef PRINT_DEBUG
	    if (get(mis, target(e, g)) != MIS_FIX0) {
	      std::cout << "vertex : "
			<< target(e, g)
			<< ", is in "
			<< get(mis, target(e, g))
			<< std::endl;
	    }
#endif

	    // cannot be MIS_UNFIX
	    assert(get(mis, target(e, g)) == MIS_FIX0);
	    found_mis = 1;
	  }
	}
#ifdef PRINT_DEBUG
	std::cout << "], ";
#endif
      } else {
	// if v is not in mis, at least one of its neighbors must
	// be in mis
	if (get(mis, v) != MIS_FIX0) {
	  std::cout << "[ERROR] Vertex - " << v << " is in " << get(mis, v)
		    << std::endl;
	}

	if (get(mis, v) != MIS_FIX0) {
	  std::cout << "Error : " << v << " is not in MIS_FIX0. But in " << get(mis, v)
		    << ", owner of " << v << ":" << get(owner, v)
		    << ", current rank : " << _RANK << std::endl;
	  BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	    auto k = target(e, g);
	    std::cout << "neigbor : " << k << " mis : " << get(mis, k)
		      << std::endl;
	  }
	}
	assert(get(mis, v) == MIS_FIX0);

	bool inmis = false;
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	  if (v == target(e, g))
	    continue;

	  if (get(mis, target(e, g)) == MIS_FIX1) {
	    inmis = true;
	    break;
	  }
	}

	if (!inmis) {
	  std::cout << "Vertex : " << v << " and none of its neighbors is in MIS" << std::endl;
	  assert(false);
	}
      }
    }
#ifdef PRINT_DEBUG
    std::cout << "}" << std::endl;
#endif

    // Run a reduction on found mis.
    // We cannot expect every rank will have found_mis true.
    // E.g:- rank0 - {0,1}, rank1 - {281474976710656, 281474976710657}
    // If every vertex is connect to each other, a vertex (Lets say 0) will
    // be in one rank. In the second rank all vertices are out of mis.
    int or_found_mis = 0;
    MPI_Allreduce(&found_mis, &or_found_mis, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

    if (or_found_mis == 0) {
      std::cout << "Did not find any MIS" <<std::endl;
      result = false;
    }

    return result;
  }

  // Luby Algorithms
  template <typename SelectGenerator, typename Graph,
	    typename graph_create_params,
	    typename MessageGenerator>
  time_type
  run_luby_maximal_is(amplusplus::transport& trans,
		    amplusplus::transport& barrier_trans,
		    Graph& g,
		    MessageGenerator msg_gen,
		    graph_create_params& gparams,
		    instance_params& runtime_params,
		    mis_instance_params& mis_params) {

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::graph_traits<Graph>::vertices_size_type vertices_size_type;

    typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
    typedef boost::iterator_property_map<typename std::vector<random_t>::iterator, VertexIndexMap>  RandomMap;

    std::vector<state_t> misvec(num_vertices(g), MIS_UNFIX);
    typedef boost::iterator_property_map<typename std::vector<state_t>::iterator, VertexIndexMap>  MISMap;
    MISMap mis(misvec.begin(), get(boost::vertex_index, g));

    // create a property map
    std::vector<random_t> pivec(num_vertices(g), 0);
    RandomMap rmap(pivec.begin(), get(boost::vertex_index, g)); // TODO remove copying

    if (trans.rank() == 0)
      std::cout << "Initializing mis map ..." << std::endl;

    BGL_FORALL_VERTICES_T(v, g, Graph)
      { put(mis, v, MIS_UNFIX); }

    // TODO find why we need this ?
    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Creating algorithm instance ..." << std::endl;

    boost::graph::distributed::luby_mis<Graph, MISMap, RandomMap,
					SelectGenerator,
					MessageGenerator>
      D(g, mis, rmap, gparams.n, trans, sched_getcpu(), msg_gen);

    trans.set_nthreads(1);

    { amplusplus::scoped_epoch epoch(barrier_trans); }

    // Many threads now
    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Invoking algorithm ..." << std::endl;

    boost::scoped_array<boost::thread> threads(new boost::thread[runtime_params.threads - 1]);
    for (int i = 0; i < runtime_params.threads - 1; ++i) {
      boost::thread thr(boost::ref(D), i + 1);
      threads[i].swap(thr);
    }

    D(0);

    for (int i = 0; i < runtime_params.threads - 1; ++i)
      threads[i].join();

    time_type end = get_time();

    if (trans.rank() == 0)
      std::cout << "Algorithm done ..." << std::endl;

#ifdef MIS_STATS
    D.print_stats();
#endif

    time_type start = D.get_start_time();

    // Back to one thread
    trans.set_nthreads(1);
    clear_thread_core_data();

    if (mis_params.verify) {
      if (trans.rank()==0)
	std::cout << "Verifying mis ..." << std::endl;

      if (!verify_mis(trans, g, mis)) {
	std::cout << "MIS Verification Failed" << std::endl;
	assert(false);
	return 0;
      }
    }

    vertices_size_type visited = 0;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (get(mis, v) != MIS_UNFIX)
	++visited;
    }

    boost::parallel::all_reduce<vertices_size_type, std::plus<vertices_size_type> >
      r(trans, std::plus<vertices_size_type>());
    vertices_size_type total = r(visited);

    if (mis_params.verify)
      if (trans.rank() == 0)
	std::cout << "Visited " << total << " vertices of " << gparams.n << " in " << print_time(end - start)
		  << std::endl;

    return end - start;
  }

  // FIX algorithms
  template <typename Graph, typename IdDistribution, typename MessageGenerator,
	    typename graph_create_params,
	    typename PriorityQueueGenerator = boost::graph::distributed::thread_priority_queue_gen>
  time_type
  run_fix_mis(amplusplus::transport& trans,
	      amplusplus::transport& barrier_trans,
	      Graph& g,
	      const IdDistribution& idd,
	      MessageGenerator msg_gen,
	      graph_create_params& gparams,
	      instance_params& runtime_params,
	      mis_instance_params& mis_params) {

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::graph_traits<Graph>::vertices_size_type vertices_size_type;

#ifdef MIS_DISTANCE_ORDERING
    std::cout << "Running with distance ordering ..." << std::endl;
#endif

    if (trans.rank() == 0)
      std::cout << "Initializing mis map ..." << std::endl;

    std::vector<state_t> misvec(num_vertices(g), MIS_UNFIX);
    typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
    typedef boost::iterator_property_map<typename std::vector<state_t>::iterator, VertexIndexMap>  MISMap;
    MISMap mis(misvec.begin(), get(boost::vertex_index, g));

    BGL_FORALL_VERTICES_T(v, g, Graph)
      { put(mis, v, MIS_UNFIX); }

    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Creating algorithm instance ..." << std::endl;

    boost::graph::distributed::maximal_independent_set<Graph, MISMap, IdDistribution,
						       PriorityQueueGenerator, MessageGenerator>
      D(g, mis, trans, idd, runtime_params.threads, sched_getcpu(), msg_gen);

    trans.set_nthreads(1);

    { amplusplus::scoped_epoch epoch(barrier_trans); }

    // Many threads now
    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Invoking algorithm ..." << std::endl;

    boost::scoped_array<boost::thread> threads(new boost::thread[runtime_params.threads - 1]);
    for (int i = 0; i < runtime_params.threads - 1; ++i) {
      boost::thread thr(boost::ref(D), i + 1);
      threads[i].swap(thr);
    }

    D(0);

    for (int i = 0; i < runtime_params.threads - 1; ++i)
      threads[i].join();

    time_type end = get_time();

    if (trans.rank() == 0)
      std::cout << "Algorithm done ..." << std::endl;

    time_type start = D.get_start_time();

    // Back to one thread
    trans.set_nthreads(1);
    clear_thread_core_data();

#ifdef MIS_PRIORITY
#ifdef MIS_STATS
    D.print_pq_sizes();
#endif
#endif

#ifdef MIS_STATS
    D.print_stats();
#endif

    if (mis_params.verify) {
      if (trans.rank()==0)
	std::cout << "Verifying mis ..." << std::endl;

      if (!verify_mis(trans, g, mis)) {
	std::cout << "MIS Verification Failed" << std::endl;
	assert(false);
	return 0;
      }
    }

    vertices_size_type visited = 0;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (get(mis, v) != MIS_UNFIX)
	++visited;
    }

    boost::parallel::all_reduce<vertices_size_type, std::plus<vertices_size_type> >
      r(trans, std::plus<vertices_size_type>());
    vertices_size_type total = r(visited);

    if (mis_params.verify)
      if (trans.rank() == 0)
	std::cout << "Visited " << total << " vertices of " << gparams.n << " in " << print_time(end - start)
		  << std::endl;

    //if (total < 100) return -1.;

    return end - start;
  }

  // FIX-Bucket algorithm
  template <typename Graph, typename IdDistribution,
	    typename graph_create_params,
	    typename MessageGenerator>
  time_type
  run_fix_mis_bucket(amplusplus::transport& trans,
		     amplusplus::transport& barrier_trans,
		     Graph& g,
		     const IdDistribution& idd,
		     MessageGenerator msg_gen,
		     graph_create_params& gparams,
		     instance_params& runtime_params,
		     mis_instance_params& mis_params) {

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::graph_traits<Graph>::vertices_size_type vertices_size_type;

    std::vector<state_t> misvec(num_vertices(g), MIS_UNFIX);
    typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
    typedef boost::iterator_property_map<typename std::vector<state_t>::iterator, VertexIndexMap>  MISMap;
    MISMap mis(misvec.begin(), get(boost::vertex_index, g));

    if (trans.rank() == 0)
      std::cout << "Initializing mis-delta map ..." << std::endl;

    BGL_FORALL_VERTICES_T(v, g, Graph)
      { put(mis, v, MIS_UNFIX); }

    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Creating algorithm instance ..." << std::endl;

    boost::graph::distributed::maximal_independent_set_delta<Graph, MISMap, IdDistribution,
							     append_buffer<Vertex, 10u>, MessageGenerator>
      D(g, mis, trans, idd, runtime_params.flush, sched_getcpu(), msg_gen);

    trans.set_nthreads(1);

    { amplusplus::scoped_epoch epoch(barrier_trans); }

    // Many threads now
    trans.set_nthreads(runtime_params.threads);

    if (trans.rank() == 0)
      std::cout << "Invoking algorithm ..." << std::endl;

    boost::scoped_array<boost::thread> threads(new boost::thread[runtime_params.threads - 1]);
    for (int i = 0; i < runtime_params.threads - 1; ++i) {
      boost::thread thr(boost::ref(D), i + 1);
      threads[i].swap(thr);
    }

    D(0);

    for (int i = 0; i < runtime_params.threads - 1; ++i)
      threads[i].join();

    time_type end = get_time();

    if (trans.rank() == 0)
      std::cout << "Algorithm done ..." << std::endl;

    time_type start = D.get_start_time();

#ifdef MIS_STATS
    D.print_stats();
#endif

    // Back to one thread
    trans.set_nthreads(1);
    clear_thread_core_data();

    if (mis_params.verify) {
      if (trans.rank()==0)
	std::cout << "Verifying delta mis ..." << std::endl;

      if (!verify_mis(trans, g, mis)) {
	std::cout << "Bucket-MIS Verification Failed" << std::endl;
	assert(false);
	return 0;
      }
    }

    vertices_size_type visited = 0;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (get(mis, v) != MIS_UNFIX)
	++visited;
    }

    boost::parallel::all_reduce<vertices_size_type, std::plus<vertices_size_type> >
      r(trans, std::plus<vertices_size_type>());
    vertices_size_type total = r(visited);

    if (mis_params.verify)
      if (trans.rank() == 0)
	std::cout << "Visited " << total << " vertices of " << gparams.n << " in " << print_time(end - start)
		  << std::endl;

    return end - start;
  }

public:
  template <typename Graph, typename MessageGenerator,
	    typename graph_create_params>
  time_type operator()(const Graph& g,
		       amplusplus::transport& trans,
		       MessageGenerator& msg_gen,
		       graph_create_params& gparams,
		       instance_params& runtime_params,
		       mis_instance_params& mis_params) { // remove

    if (mis_params.algorithm == fix_bucket) {
      amplusplus::transport barrier_trans = trans.clone();
      if (mis_params.id_distribution == vertical) {
	return run_fix_mis_bucket(trans,
				  barrier_trans,
				  g,
				  block_id_distribution<Graph>(g, gparams.n),
				  msg_gen,
				  gparams,
				  runtime_params,
				  mis_params);
      } else if (mis_params.id_distribution == horizontal) {
	return run_fix_mis_bucket(trans,
				  barrier_trans,
				  g,
				  row_id_distribution<Graph>(g, trans.size()),
				  msg_gen,
				  gparams,
				  runtime_params,
				  mis_params);

      } else {
	std::cerr << "Invalid id distribution ! " << std::endl;
	assert(false);
      }

    }

    if (mis_params.algorithm == fix) {
      amplusplus::transport barrier_trans = trans.clone();

      if (mis_params.id_distribution == vertical) {
	return run_fix_mis(trans,
			   barrier_trans,
			   g,
			   block_id_distribution<Graph>(g, gparams.n),
			   msg_gen,
			   gparams,
			   runtime_params,
			   mis_params); //remove
      } else if (mis_params.id_distribution == horizontal) {
	return run_fix_mis(trans,
			   barrier_trans,
			   g,
			   row_id_distribution<Graph>(g, trans.size()),
			   msg_gen,
			   gparams,
			   runtime_params,
			   mis_params); //remove

      } else {
	std::cerr << "Invalid id distribution ! " << std::endl;
	assert(false);
      }
    }

    if (mis_params.algorithm == luby_a) {
      amplusplus::transport barrier_trans = trans.clone();
      return run_luby_maximal_is<boost::graph::distributed::select_a_functor_gen>(trans,
										  barrier_trans,
										  g,
										  msg_gen,
										  gparams,
										  runtime_params,
										  mis_params);

    }

    if (mis_params.algorithm == luby_av1) {
      amplusplus::transport barrier_trans = trans.clone();
      return run_luby_maximal_is<boost::graph::distributed::select_a_vertex_functor_gen>(trans,
										  barrier_trans,
										  g,
										  msg_gen,
										  gparams,
										  runtime_params,
										  mis_params);

    }

    if (mis_params.algorithm == luby_av2) {
      amplusplus::transport barrier_trans = trans.clone();
      return run_luby_maximal_is<boost::graph::distributed::select_a_v2_functor_gen>(trans,
										  barrier_trans,
										  g,
										  msg_gen,
										  gparams,
										  runtime_params,
										  mis_params);

    }

    if (mis_params.algorithm == luby_b) {
      amplusplus::transport barrier_trans = trans.clone();
      return run_luby_maximal_is<boost::graph::distributed::select_b_functor_gen>(trans,
										  barrier_trans,
										  g,
										  msg_gen,
										  gparams,
										  runtime_params,
										  mis_params);

    }

  }

};

int main(int argc, char* argv[]) {
  std::cout << "printing core id for process ..." << std::endl;
  print_core_id();

  executor<MISExecutor, mis_params, mis_instance_params> mis_executor;
  mis_executor.execute(argc, argv);

}


//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
//

/*
  This file implements the function

  template <class EdgeListGraph, class Size, class P, class T, class R>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N,
     const bgl_named_params<P, T, R>& params)

 */

#ifndef BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/named_function_params.hpp>

namespace boost {

  template <class Visitor, class Graph>
  struct BellmanFordVisitorConcept {
    void constraints() {
      function_requires< CopyConstructibleConcept<Visitor> >();
      vis.examine_edge(e, g);
      vis.edge_relaxed(e, g);
      vis.edge_not_relaxed(e, g);
      vis.edge_minimized(e, g);
      vis.edge_not_minimized(e, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits<Graph>::edge_descriptor e;
  };

  template <class Visitors = null_visitor>
  class bellman_visitor {
  public:
    bellman_visitor() { }
    bellman_visitor(Visitors vis) : m_vis(vis) { }

    template <class Edge, class Graph>
    void examine_edge(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_examine_edge());
    }
    template <class Edge, class Graph>
    void edge_relaxed(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_relaxed());
    }
    template <class Edge, class Graph>
    void edge_not_relaxed(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_not_relaxed());
    }
    template <class Edge, class Graph>
    void edge_minimized(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_minimized());
    }
    template <class Edge, class Graph>
    void edge_not_minimized(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_not_minimized());
    }
  protected:
    Visitors m_vis;
  };
  template <class Visitors>
  bellman_visitor<Visitors>
  make_bellman_visitor(Visitors vis) {
    return bellman_visitor<Visitors>(vis);
  }
  typedef bellman_visitor<> default_bellman_visitor;

  template <class EdgeListGraph, class Size, class WeightMap,
            class PredecessorMap, class DistanceMap,
            class BinaryFunction, class BinaryPredicate,
            class BellmanFordVisitor>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N,
                         WeightMap weight,
                         PredecessorMap pred,
                         DistanceMap distance,
                         BinaryFunction combine,
                         BinaryPredicate compare,
                         BellmanFordVisitor v)
  {
    function_requires<EdgeListGraphConcept<EdgeListGraph> >();
    typedef graph_traits<EdgeListGraph> GTraits;
    typedef typename GTraits::edge_descriptor Edge;
    typedef typename GTraits::vertex_descriptor Vertex;
    function_requires<ReadWritePropertyMapConcept<DistanceMap, Vertex> >();
    function_requires<ReadablePropertyMapConcept<WeightMap, Edge> >();
    typedef typename property_traits<DistanceMap>::value_type D_value;
    typedef typename property_traits<WeightMap>::value_type W_value;

    typename GTraits::edge_iterator i, end;

    for (Size k = 0; k < N; ++k) {
      bool at_least_one_edge_relaxed = false;
      for (tie(i, end) = edges(g); i != end; ++i) {
        v.examine_edge(*i, g);
        if (relax(*i, g, weight, pred, distance, combine, compare)) {
          at_least_one_edge_relaxed = true;
          v.edge_relaxed(*i, g);
        } else
          v.edge_not_relaxed(*i, g);
      }
      if (!at_least_one_edge_relaxed)
        break;
    }

    for (tie(i, end) = edges(g); i != end; ++i)
      if (compare(combine(get(distance, source(*i, g)),
                          get(weight, *i)),
                  get(distance, target(*i,g))))
      {
        v.edge_not_minimized(*i, g);
        return false;
      } else
        v.edge_minimized(*i, g);

    return true;
  }

  namespace detail {

    template <class EdgeListGraph, class Size, class WeightMap,
              class DistanceMap, class P, class T, class R>
    bool bellman_dispatch(EdgeListGraph& g, Size N,
                          WeightMap weight, DistanceMap distance,
                          const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      bellman_visitor<> null_vis;
      dummy_property_map dummy_pred;
      return bellman_ford_shortest_paths
        (g, N, weight,
         choose_param(get_param(params, vertex_predecessor), dummy_pred),
         distance,
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<D>()),
         choose_param(get_param(params, distance_compare_t()),
                      std::less<D>()),
         choose_param(get_param(params, graph_visitor),
                       null_vis)
         );
    }

  } // namespace detail

  template <class EdgeListGraph, class Size, class P, class T, class R>
  bool bellman_ford_shortest_paths
    (EdgeListGraph& g, Size N,
     const bgl_named_params<P, T, R>& params)
  {
    return detail::bellman_dispatch
      (g, N,
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
       params);
  }

  template <class EdgeListGraph, class Size>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N)
  {
    bgl_named_params<int,int> params(0);
    return bellman_ford_shortest_paths(g, N, params);
  }

} // namespace boost

#endif // BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   Material.hh
 *  @author Jeremy Roberts
 *  @brief  Material class definition.
 */
//---------------------------------------------------------------------------//

#ifndef detran_material_MATERIAL_HH_
#define detran_material_MATERIAL_HH_

#include "material/material_export.hh"
#include "utilities/Definitions.hh"
#include "utilities/SP.hh"
#include <string>
#ifdef DETRAN_ENABLE_BOOST
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#endif

namespace detran_material
{

//---------------------------------------------------------------------------//
/**
 *  @class Material
 *  @brief Simple cross section container.
 *
 *  All data is stored with the material index changing fastest.  This
 *  appears to be the best storage scheme with respect to memory access.
 */
//---------------------------------------------------------------------------//
class MATERIAL_EXPORT Material
{

public:

  //-------------------------------------------------------------------------//
  // TYPEDEFS
  //-------------------------------------------------------------------------//

  typedef detran_utilities::SP<Material> SP_material;
  typedef detran_utilities::vec_dbl      vec_dbl;
  typedef detran_utilities::vec2_dbl     vec2_dbl;
  typedef detran_utilities::vec3_dbl     vec3_dbl;
  typedef detran_utilities::vec_int      vec_int;
  typedef detran_utilities::vec2_int     vec2_int;
  typedef detran_utilities::vec_size_t   vec_size_t;
  typedef detran_utilities::vec2_size_t  vec2_size_t;
  typedef detran_utilities::size_t       size_t;

  //-------------------------------------------------------------------------//
  // PUBLIC INTERFACE
  //-------------------------------------------------------------------------//

  /**
   *  @brief Constructor.
   *  @param    number_materials    Number of materials.
   *  @param    number_groups       Number of energy groups.
   *  @param    downscatter         Switch on to use only downscatter.
   */
  Material(const size_t number_materials,
           const size_t number_groups,
           std::string  name = "no name given");

  /// Virtual destructor
  virtual ~Material(){}

  /// SP constructor
  static SP_material Create(const size_t number_materials,
                            const size_t number_groups,
                            std::string  name = "no name given");

  //--------------------------------------------------------------------------//
  // Setters
  //--------------------------------------------------------------------------//

  /**
   *  @brief Explicitly turn on downscatter-only
   */
  void set_downscatter(bool v, bool tran = false)
  {
    if (tran) d_downscatter[1] = v;
    d_downscatter[0] = v;
    if (d_finalized) finalize();
  }

  void set_sigma_t(size_t m, size_t g, double v);
  void set_sigma_a(size_t m, size_t g, double v);
  void set_nu_sigma_f(size_t m, size_t g, double v);
  void set_sigma_f(size_t m, size_t g, double v);
  void set_nu(size_t m, size_t g, double v);
  void set_chi(size_t m, size_t g, double v);
  void set_sigma_s(size_t m, size_t g, size_t gp, double v);
  void set_diff_coef(size_t m, size_t g, double v);

  // Vectorized setters

  void set_sigma_t(size_t m, vec_dbl &v);
  void set_sigma_a(size_t m, vec_dbl &v);
  void set_nu_sigma_f(size_t m, vec_dbl &v);
  void set_sigma_f(size_t m, vec_dbl &v);
  void set_nu(size_t m, vec_dbl &v);
  void set_chi(size_t m, vec_dbl &v);
  void set_sigma_s(size_t m, size_t g, vec_dbl &v);
  void set_diff_coef(size_t m, vec_dbl &v);

  //------------------------------------------------------------------------//
  // Getters
  //------------------------------------------------------------------------//

  virtual double sigma_t(size_t m, size_t g) const;
  virtual double sigma_a(size_t m, size_t g) const;
  virtual double nu_sigma_f(size_t m, size_t g) const;
  virtual double sigma_f(size_t m, size_t g) const;
  virtual double nu(size_t m, size_t g) const;
  virtual double chi(size_t m, size_t g) const;
  virtual double sigma_s(size_t m, size_t g, size_t gp) const;
  virtual double diff_coef(size_t m, size_t g) const;

  // Vectorized getters

  virtual vec_dbl sigma_t(size_t m) const;
  virtual vec_dbl sigma_a(size_t m) const;
  virtual vec_dbl nu_sigma_f(size_t m) const;
  virtual vec_dbl sigma_f(size_t m) const;
  virtual vec_dbl nu(size_t m) const;
  virtual vec_dbl chi(size_t m) const;
  virtual vec2_dbl sigma_s(size_t m) const;
  virtual vec_dbl diff_coef(size_t m) const;

  //------------------------------------------------------------------------//
  // OTHER ACCESSORS
  //------------------------------------------------------------------------//

  size_t number_groups() const
  {
    return d_number_groups;
  }

  size_t number_materials() const
  {
    return d_number_materials;
  }

  /**
   *  @brief Lower scatter group bound.
   *
   *  This is the *lowest* index (highest energy) \f$ g' \f$
   *  that leads to downscatter for a given outgoing group \f$ g \f$.
   *
   *  @param g        Row of the scattering matrix
   *  @param tran     Flag for accessing transpose of S
   */
  size_t lower(size_t g, bool tran = false) const;

  /**
   *  @brief Upper scatter group bound.
   *
   *  This is the *highest* index (lowest energy) \f$ g' \f$
   *  that upscatters size_to the outgoing group \f$ g \f$.
   *
   *  @param g        Row of the scattering matrix
   *  @param tran     Flag for accessing transpose of S
   */
  size_t upper(size_t g, bool tran = false) const;

  /// Do we do only downscatter?
  bool downscatter(bool tran = false) const;

  /**
   *  @brief Index below which upscatter doesn't occur for any material.
   *
   *  For adjoint problems, this is the group above which
   */
  size_t upscatter_cutoff(bool tran = false) const;

  /**
   *  @brief Compute the absorption cross section from total and scattering.
   *  @note this overwrites any data for \f$ \Sigma_a \f$ already stored.
   */
  void compute_sigma_a();

  /**
   *  @brief Compute the diffusion coefficient from \f$ \Sigma_t \f$.
   *
   *  Assuming isotropic scattering in the LAB, the diffusion
   *  coefficient is simply \f$ D = 1/3\Sigma_t \f$.
   *
   *  @todo Update diffusion definition if anisotropic scattering
   *        is added.
   *  @note This overwrites any data for \f$ D \f$ already stored.
   */
  void compute_diff_coef();

  /// Computes scattering bounds and absorption cross section.
  void finalize();

  /// Pretty print the material database.
  virtual void display();

protected:

  //-------------------------------------------------------------------------//
  // DATA
  //-------------------------------------------------------------------------//

  /// Material name
  std::string d_name;
  /// Number of groups
  size_t d_number_groups;
  /// Number of materials
  size_t d_number_materials;
  /// Downscatter switch (when true, upscatter ignored)
  bool d_downscatter[2];
  /// Total cross section [material, group]
  vec2_dbl d_sigma_t;
  /// Absorption cross section [material, group]
  vec2_dbl d_sigma_a;
  /// nu * Fission [material, group]
  vec2_dbl d_nu_sigma_f;
  /// Fission [material, group]
  vec2_dbl d_sigma_f;
  /// nu [material, group]
  vec2_dbl d_nu;
  /// Fission spectrum [material, group]
  vec2_dbl d_chi;
  /// Scatter [material, group<-, group']
  vec3_dbl d_sigma_s;
  /// Diffusion coefficient [material, group]
  vec2_dbl d_diff_coef;
  /// Scatter bounds applied to all materials [group, 2]
  vec2_size_t d_scatter_bounds;
  /// Groups equal to or above cutoff are subject to upscatter iterations
  size_t d_upscatter_cutoff[2];
  /// Are we ready to be used?
  bool d_finalized;

  //-------------------------------------------------------------------------//
  // IMPLEMENTATION
  //-------------------------------------------------------------------------//

  void material_display();

#ifdef DETRAN_ENABLE_BOOST

  /// Default constructor needed for serialization
  Material(){}

  friend class boost::serialization::access;

  template<class Archive>
  void serialize(Archive & ar, const unsigned int version)
  {
    ar & d_number_groups;
    ar & d_number_materials;
    ar & d_downscatter;
    ar & d_sigma_t;
    ar & d_sigma_a;
    ar & d_nu_sigma_f;
    ar & d_sigma_f;
    ar & d_nu;
    ar & d_chi;
    ar & d_sigma_s;
    ar & d_diff_coef;
    ar & d_scatter_bounds;
    ar & d_upscatter_cutoff;
    ar & d_finalized;
  }

#endif

};

MATERIAL_TEMPLATE_EXPORT(detran_utilities::SP<Material>)

} // end namespace detran_material

//---------------------------------------------------------------------------//
// INLINE FUNCTIONS
//---------------------------------------------------------------------------//

#include "Material.i.hh"

#endif /* detran_material_MATERIAL_HH_ */