/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * 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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "xregLineSearchOpt.h"

#include <boost/variant.hpp>

#include <fmt/printf.h>

#include "xregAssert.h"

namespace
{

using namespace xreg;

using Pt  = LineSearchOptimization::Pt;
using Mat = LineSearchOptimization::Mat;

using SearchDirFnFirstOrder = LineSearchOptimization::SearchDirFnFirstOrder;
using SearchDirFnSecOrder   = LineSearchOptimization::SearchDirFnSecOrder;

struct SearchDirNeedsHessian : public boost::static_visitor<bool>
{
  bool operator()(const SearchDirFnFirstOrder& search_fn) const
  {
    xregASSERT(bool(search_fn));
    return false;
  }

  bool operator()(const SearchDirFnSecOrder& search_fn) const
  {
    xregASSERT(bool(search_fn));
    return true;
  }
};

struct ComputeSearchDir : public boost::static_visitor<Pt>
{
  const Pt*  g;
  const Mat* H;

  Pt operator()(const SearchDirFnFirstOrder& search_fn) const
  {
    return search_fn(*g);
  }

  Pt operator()(const SearchDirFnSecOrder& search_fn) const
  {
    return search_fn(*g, *H);
  }
};

}  // un-named

std::tuple<xreg::LineSearchOptimization::TermStatus,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::size_type>
xreg::LineSearchOptimization::solve(const Pt& init_x) const
{
  xregASSERT(bool(obj_fn));
  xregASSERT(bool(backtrack_fn));

  TermStatus status = kOTHER;

  Scalar cur_F;
  Pt cur_x = init_x;
  Pt cur_grad;
  Mat cur_H;

  // search direction
  Pt p;

  Pt prev_x;

  const bool requires_hessian = boost::apply_visitor(SearchDirNeedsHessian(), search_dir_fn);

  std::tie(cur_F,cur_grad,cur_H) = obj_fn(cur_x, true, requires_hessian);

  Scalar cur_grad_norm = cur_grad.norm();

  const Scalar grad_term_tol = std::max(Scalar(1), cur_grad_norm) * grad_tol;

  bool should_stop = false;

  size_type cur_it = 0;

  dout() << " Iter.      F       ||grad F||" << std::endl;

  while (!should_stop)
  {
    dout() << fmt::sprintf("%04lu   %+10.4f  %+10.4f", cur_it, cur_F, cur_grad_norm) << std::endl;

    if (cur_grad_norm <= grad_term_tol)
    {
      status = kGRAD_CONVERGED;
      should_stop = true;
    }
    else if ((cur_it > 0) && ((prev_x - cur_x).norm() < param_tol))
    {
      status = kNO_POS_CHANGE;
      should_stop = true;
    }
    else if (max_its && (cur_it >= max_its))
    {
      status = kMAX_ITS_PERFORMED;
      should_stop = true;
    }
    else
    {
      {
        ComputeSearchDir compute_search_dir_visitor;
        compute_search_dir_visitor.g = &cur_grad;
        compute_search_dir_visitor.H = &cur_H;

        p = boost::apply_visitor(compute_search_dir_visitor, search_dir_fn);
      }

      prev_x = cur_x;

      std::tie(cur_x,cur_F,cur_grad,cur_H) = backtrack_fn(obj_fn, p, prev_x, cur_F, cur_grad, requires_hessian);

      cur_grad_norm = cur_grad.norm();

      ++cur_it;
    }
  }

  dout() << "Termination Status: " << TermStatusString(status) << std::endl;

  return std::make_tuple(status, cur_x, cur_F, cur_it);
}

std::string xreg::LineSearchOptimization::TermStatusString(const TermStatus status)
{
  std::string s;

  switch (status)
  {
  case kGRAD_CONVERGED:
    s = "Gradient Converged";
    break;
  case kMAX_ITS_PERFORMED:
    s = "Maximum Iterations Performed";
    break;
  case kNO_POS_CHANGE:
    s = "Insufficient Parameter Change";
    break;
  case kOTHER:
  default:
    s = "Other/Unknown";
    break;
  }

  return s;
}

xreg::LineSearchOptimization::Pt
xreg::NegativeGradSearchDir(const LineSearchOptimization::Pt& grad,
                            const LineSearchOptimization::Mat& /*hessian*/)
{
  return -1 * grad;
}

xreg::LineSearchOptimization::Pt
xreg::NewtonSearchDir(const LineSearchOptimization::Pt& grad,
                      const LineSearchOptimization::Mat& hessian)
{
  using Mat = LineSearchOptimization::Mat;

  // solves hessian \ -grad
  return Eigen::JacobiSVD<Mat>(hessian, Eigen::ComputeThinU | Eigen::ComputeThinV).solve(-1 * grad);
}

xreg::ModNewtonSearchDir::Pt xreg::ModNewtonSearchDir::operator()(const Pt& grad, const Mat& hessian)
{
  spectral_dcomp.compute(hessian, Eigen::ComputeEigenvectors);

  const SpectralDecomp::RealVectorType& eig_vals = spectral_dcomp.eigenvalues();

  const Scalar H_L2_norm = eig_vals.array().abs().maxCoeff();

  const Scalar eps = (H_L2_norm > 1.0e-6) ? (H_L2_norm / beta) : Scalar(1);

  bool use_hessian = true;

  const unsigned long d = static_cast<unsigned long>(grad.size());

  mod_eigen_vals.resize(d);

  for (unsigned long i = 0; i < d; ++i)
  {
    if (eig_vals(i) >= eps)
    {
      mod_eigen_vals(i) = eig_vals(i);
    }
    else if (eig_vals(i) <= -eps)
    {
      mod_eigen_vals(i) = -eig_vals(i);
      use_hessian = false;
    }
    else
    {
      mod_eigen_vals(i) = eps;
      use_hessian = false;
    }
  }

  if (!use_hessian)
  {
    const Mat& eigen_vecs = spectral_dcomp.eigenvectors();

    B = eigen_vecs * mod_eigen_vals.asDiagonal() * eigen_vecs.transpose();
  }

  // solve B \ -grad
  return NewtonSearchDir(grad, use_hessian ? hessian : B);
}

xreg::LineSearchOptimization::SearchDirFn xreg::ModNewtonSearchDir::callback_fn()
{
  return [this] (const Pt& grad, const Mat& hessian) { return this->operator()(grad, hessian); };
}

std::tuple<xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Mat>
xreg::FixedStepNoBacktracking(const LineSearchOptimization::ObjFn& obj_fn,
                              const LineSearchOptimization::Pt& p,
                              const LineSearchOptimization::Pt& x,
                              const bool compute_hessian,
                              const LineSearchOptimization::Scalar alpha)
{
  using Pt     = LineSearchOptimization::Pt;
  using Mat    = LineSearchOptimization::Mat;
  using Scalar = LineSearchOptimization::Scalar;

  Pt next_x = x + (alpha * p);

  Scalar next_F;
  Pt next_g;
  Mat next_H;

  std::tie(next_F, next_g, next_H) = obj_fn(next_x, true, compute_hessian);

  return std::make_tuple(next_x, next_F, next_g, next_H);
}

xreg::LineSearchOptimization::BacktrackFn
xreg::MakeFixedStepNoBacktrackingCallback(const LineSearchOptimization::Scalar alpha)
{
  return [alpha] (const LineSearchOptimization::ObjFn& obj_fn,
                  const LineSearchOptimization::Pt& p,
                  const LineSearchOptimization::Pt& x,
                  const LineSearchOptimization::Scalar /*F*/,
                  const LineSearchOptimization::Pt& /*g*/,
                  const bool compute_hessian)
  {
    return FixedStepNoBacktracking(obj_fn, p, x, compute_hessian, alpha);
  };
}

std::tuple<xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Mat>
xreg::BacktrackingArmijo(const LineSearchOptimization::ObjFn& obj_fn,
                         const LineSearchOptimization::Pt& p,
                         const LineSearchOptimization::Pt& x,
                         const LineSearchOptimization::Scalar F,
                         const LineSearchOptimization::Pt& g,
                         const bool compute_hessian,
                         const LineSearchOptimization::Scalar init_alpha,
                         const LineSearchOptimization::Scalar eta,
                         const LineSearchOptimization::Scalar tau)
{
  using Pt     = LineSearchOptimization::Pt;
  using Mat    = LineSearchOptimization::Mat;
  using Scalar = LineSearchOptimization::Scalar;

  const Scalar eta_grad_dot_p = eta * g.dot(p);

  Scalar alpha = init_alpha;

  Pt next_x = x + (alpha * p);

  Scalar next_F;

  // passing false for gradient and hessian defers their computation until
  // after we stop backtracking - this really helps for expensive computations

  std::tie(next_F,std::ignore,std::ignore) = obj_fn(next_x, false, false);

  while (next_F > (F + (alpha * eta_grad_dot_p)))
  {
    alpha *= tau;
    next_x = x + (alpha * p);
    std::tie(next_F,std::ignore,std::ignore) = obj_fn(next_x, false, false);
  }

  Pt next_g;

  Mat next_H;

  std::tie(next_F,next_g,next_H) = obj_fn(next_x, true, compute_hessian);

  return std::make_tuple(next_x, next_F, next_g, next_H);
}

xreg::LineSearchOptimization::BacktrackFn
xreg::MakeBacktrackingArmijoCallback(const LineSearchOptimization::Scalar alpha,
                                     const LineSearchOptimization::Scalar eta,
                                     const LineSearchOptimization::Scalar tau)
{
  return [alpha,eta,tau] (const LineSearchOptimization::ObjFn& obj_fn,
                          const LineSearchOptimization::Pt& p,
                          const LineSearchOptimization::Pt& x,
                          const LineSearchOptimization::Scalar F,
                          const LineSearchOptimization::Pt& g,
                          const bool compute_hessian)
  {
    return BacktrackingArmijo(obj_fn, p, x, F, g, compute_hessian, alpha, eta, tau);
  };
}


/**
 * \file gtpack/cooperative.hpp
 *
 * \brief Cooperative games.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2013 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef GTPACK_COOPERATIVE_HPP
#define GTPACK_COOPERATIVE_HPP

#include <boost/dynamic_bitset.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/smart_ptr.hpp>
#include <cstddef>
#include <dcs/algorithm/combinatorics.hpp>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/math/traits/float.hpp>
#include <iostream>
#include <ilconcert/iloalg.h>
#include <ilconcert/iloenv.h>
#include <ilconcert/iloexpression.h>
#include <ilconcert/ilomodel.h>
#include <ilcplex/ilocplex.h>
#include <limits>
#include <map>
#include <set>
#include <sstream>
#include <stdexcept>

namespace gtpack {

typedef unsigned int pid_type;
typedef pid_type player_type; //XXX: DEPRECATED
typedef unsigned long cid_type;

static const cid_type empty_cid = 0;
static const cid_type empty_coalition_id = empty_cid; //XXX: DEPRECATED

/// Returns the identifier of the singleton coalition for the given player
inline
cid_type make_coalition_id(pid_type pid)
{
	const cid_type one = 1;

	return one << pid;
}

/// Returns the identifier of the coalition for the given players
template <typename IterT>
cid_type make_coalition_id(IterT first_player, IterT last_player)
{
	// Each coalition $S\subseteq\{1,2,...,N\}$ can be characterized
	// uniquely by the $\sum_{i \in S}{2^{i-1}}$, which is the sum of
	// integers associated to players belonging to a particular coalition.

	const cid_type one = 1;

	cid_type cid = 0;

	while (first_player != last_player)
	{
		const pid_type pid = *first_player;

		// Update only if the player in not already member of this coalition
		if (!(cid & (one << pid)))
		{
			cid += one << pid;
		}

		++first_player;
	}

	return cid;
}

/// Returns the identifier of the grand coalition for \a n players
inline
cid_type make_grand_coalition_id(std::size_t n)
{
	if (n == 0)
	{
		return empty_cid;
	}

	const std::size_t nb = sizeof(cid_type)*8; // # bits in cid_type
	return ~static_cast<cid_type>(0) >> (nb-n);
}

/// Returns the identifier of the complement of a given coalition
inline
cid_type make_complement_coalition_id(std::size_t n, cid_type cid)
{
    return make_grand_coalition_id(n) - cid;
}

/// Returns the identifier of the complement of a coalition for the given players
template <typename IterT>
cid_type make_complement_coalition_id(std::size_t n, IterT first_player, IterT last_player)
{
	return make_complement_coalition_id(n, make_coalition_id(first_player, last_player));
}

template <typename RealT>
struct characteristic_function
{
	typedef RealT real_type;

	public: real_type operator()(cid_type cid) const
	{
		if (cid == empty_coalition_id)
		{
			return 0;
		}

		return this->do_get(cid);
	}

	public: void operator()(cid_type cid, real_type v)
	{
		DCS_ASSERT(cid != empty_coalition_id,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Cannot change the value of the empty coalition"));

		this->do_set(cid, v);
	}

//	public: real_type& operator()(cid_type cid)
//	{
//		return this->do_get(cid);
//	}

	private: virtual real_type do_get(cid_type cid) const = 0;

	private: virtual void do_set(cid_type cid, real_type v) = 0;

//	private: virtual real_type& do_get(cid_type cid) = 0;
}; // characteristic_function

template <typename RealT>
class enumerated_characteristic_function: public characteristic_function<RealT>
{
	public: typedef RealT real_type;

	public: enumerated_characteristic_function()
	{
	}

	public: template <typename CidValIterT>
			enumerated_characteristic_function(CidValIterT first, CidValIterT last)
	: map_(first, last)
	{
	}

	private: real_type do_get(cid_type cid) const
	{
		return map_.count(cid) > 0 ? map_.at(cid) : ::std::numeric_limits<real_type>::quiet_NaN();
	}

	private: void do_set(cid_type cid, real_type v)
	{
		map_[cid] = v;
	}

//	private: real_type& do_get(cid_type cid)
//	{
//		return map_[cid];
//	}

	private: ::std::map<cid_type,real_type> map_;
}; // enumerated_characteristic_function

template <typename RealT>
class explicit_characteristic_function: public enumerated_characteristic_function<RealT> { };
/*
{
	public: typedef RealT real_type;

	public: explicit_characteristic_function()
	{
	}

	public: template <typename CidValIterT>
			explicit_characteristic_function(CidValIterT first, CidValIterT last)
	: map_(first, last)
	{
	}

	private: real_type do_get(cid_type cid) const
	{
		return map_.count(cid) > 0 ? map_.at(cid) : ::std::numeric_limits<real_type>::quiet_NaN();
	}

	private: void do_set(cid_type cid, real_type v)
	{
		map_[cid] = v;
	}

//	private: real_type& do_get(cid_type cid)
//	{
//		return map_[cid];
//	}

	private: ::std::map<cid_type,real_type> map_;
}; // explicit_characteristic_function
*/

template <typename RealT>
class players_coalition
{
	public: typedef unsigned long cid_type;
	public: typedef RealT real_type;
	private: typedef ::boost::dynamic_bitset<> bitset_container;
	private: typedef typename bitset_container::size_type size_type;

//	template <typename C, typename CT, typename R>
//	friend
//	::std::basic_ostream<C,CT>& operator<<(::std::basic_ostream<C,CT>&, players_coalition<R> const&);

	public: template <typename IterT>
			static cid_type make_id(IterT first_player, IterT last_player)
	{
		// Each coalition $S\subseteq\{1,2,...,N\}$ can be characterized
		// uniquely by the $\sum_{i \in S}{2^{i-1}}$, which is the sum of
		// integers associated to players belonging to a particular coalition.

		const cid_type one(1);

		cid_type cid(0);

		while (first_player != last_player)
		{
			cid += one << *first_player;

			++first_player;
		}

		return cid;
	}

	public: players_coalition(::std::size_t n, cid_type cid)
	: id_(cid),
	  players_(),
	  //players_bs_(n, id_),
	  players_bs_(::std::max(num_bits(id_), n), id_),
	  n_(players_bs_.count()),
	  v_(0)
	{
		for (size_type player = players_bs_.find_first();
			 player != bitset_container::npos;
			 player = players_bs_.find_next(player))
		{
			players_.push_back(player);
		}
	}

//	public: template <typename IterT>
//			players_coalition(::std::size_t n, IterT first_player, IterT last_player)
//	: id_(make_id(first_player, last_player)),
//	  players_(n, id_),
//	  n_(players_.count()),
//	  v_(0)
//	{
//	}

	public: template <typename IterT>
			players_coalition(IterT first_player, IterT last_player)
	: id_(make_id(first_player, last_player)),
	  players_(first_player, last_player),
	  players_bs_(players_.size(), id_),
	  n_(players_.size()),
	  v_(0)
	{
	}

	public: cid_type id() const
	{
		return id_;
	}

	public: ::std::size_t size() const
	{
		return n_;
	}

	public: bool has_player(pid_type player) const
	{
		return players_bs_.test(player);
	}

	public: ::std::vector<pid_type> players() const
	{
		return players_;
	}

	public: ::std::size_t num_players() const
	{
		return players_.size();
	}

	public: void value(real_type v)
	{
		v_ = v;
	}

	public: real_type value() const
	{
		return v_;
	}

//	private: static unsigned int count_bit_set(unsigned long v)
//	{
//		unsigned int c(0); // c accumulates the total bits set in v
//		while (v)
//		{
//			v &= v-1; // Clear the least significant bit set
//			++c;
//		}
//
//		return c;
//	}

	private: static ::std::size_t num_bits(unsigned long v)
	{
		return ::std::floor(::std::log(v)/::std::log(2)+1);
	}

	//private: ::std::bitset<8*sizeof(cid_type)> players_;
	private: cid_type id_;
	private: ::std::vector<pid_type> players_;
	private: bitset_container players_bs_;
	private: ::std::size_t n_;
	private: RealT v_;
}; // players_coalition

template <typename CharT,
		  typename CharTraitsT,
		  typename RealT>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, players_coalition<RealT> const& coalition)
{
	typedef ::std::vector<pid_type> player_container;
	typedef typename player_container::const_iterator player_iterator;

	bool first(true);

	os << "{";
	player_container players(coalition.players());
	player_iterator end_it(players.end());
	for (player_iterator it = players.begin(); it != end_it; ++it)
	{
		if (!first)
		{
			os << ",";
		}
		else
		{
			first = false;
		}
		os << *it;
	}
	os << "}";

	return os;
}

template <typename RealT>
class cooperative_game
{
	public: typedef RealT real_type;

	public: cooperative_game()
	{
	}

	public: cooperative_game(::std::size_t n,
							 ::boost::shared_ptr< characteristic_function<RealT> > const& p_v)
	: n_(n),
	  p_v_(p_v)
	{
		for (pid_type i = 0; i < n_; ++i)
		{
			players_.insert(i);
		}

		coal_struc_.insert(this->coalition(players_.begin(), players_.end()).id());
	}

	public: template <typename IterT>
			cooperative_game(IterT first_player,
							 IterT last_player,
							 ::boost::shared_ptr< characteristic_function<RealT> > const& p_v)
	: n_(0),
	  p_v_(p_v),
	  players_(first_player,last_player)
	{
		n_ = players_.size();

		coal_struc_.insert(this->coalition(players_.begin(), players_.end()).id());
	}

	public: ::std::size_t num_players() const
	{
		return n_;
	}

	public: ::std::vector<pid_type> players() const
	{
		return ::std::vector<pid_type>(players_.begin(), players_.end());
	}

	public: real_type value(cid_type cid) const
	{
		return (*p_v_)(cid);
	}

	public: void value(cid_type cid, real_type value)
	{
		(*p_v_)(cid, value);
	}

	public: players_coalition<real_type> coalition(cid_type cid) const
	{
		players_coalition<real_type> c(n_, cid);

		c.value(this->value(cid));

		return c;
	}

	public: template <typename PlayerIterT>
			players_coalition<real_type> coalition(PlayerIterT first, PlayerIterT last) const
	{
		players_coalition<real_type> c(first, last);

		c.value(this->value(c.id()));

		return c;
	}

	public: template <typename CidIterT>
			void coalition_structure(CidIterT first, CidIterT last)
	{
		coal_struc_.clear();
		while (first != last)
		{
			const cid_type cid(*first);

			coal_struc_.insert(cid);

			++first;
		}
	}

//	public: ::std::vector<cid_type> coalition_structure() const
//	{
//		return ::std::vector<cid_type>(coal_struc_.begin(), coal_struc_.end());
//	}
	public: ::std::set<cid_type> coalition_structure() const
	{
		return coal_struc_;
	}

	public: template <typename PlayerIterT>
			cooperative_game<real_type> subgame(PlayerIterT first, PlayerIterT last) const
	{
		return cooperative_game<real_type>(first, last, p_v_);
	}

	public: cooperative_game<real_type> subgame(cid_type cid) const
	{
		const ::std::vector<pid_type> players = this->coalition(n_, cid).players();

		return this->subgame(players.begin(), players.end());
	}

	private: ::std::size_t n_; ///< Number of players
	//private: ::boost::function<real_type (cid_type)> p_v_; ///< Pointer to the characteristic function
	private: ::boost::shared_ptr< characteristic_function<real_type> > p_v_; ///< Pointer to the characteristic function
	private: ::std::set<pid_type> players_; ///< The set of players
	private: ::std::set<cid_type> coal_struc_; ///< The set of coalition ID making the coalition structure for this game
}; // cooperative_game

template <typename RealT>
class core
{
	public: typedef RealT real_type;

	public: core(/*::boost::shared_ptr< cooperative_game<real_type> > const& p_game*/)
	: empty_(true)
	{
	}

	public: template <typename IterT>
			core(IterT first_payoff, IterT last_payoff)
	: empty_(false),
	  x_(first_payoff, last_payoff)
	{
	}

	public: ::std::vector<real_type> imputation() const
	{
		return x_;
	}

	public: bool empty() const
	{
		return empty_;
	}

	private: bool empty_;
	private: ::std::vector<real_type> x_;
};

/**
 * \brief Compute the Shapley value for a given player.
 *
 * Given a game \f$(N,v)\f$, the Shapley value \f$\phi_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \phi_i(v)=\sum_{S \subseteq N \setminus \{i\}} \frac{|S|!\; (|N|-|S|-1)!}{|N|!}(v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
RealT shapley_value(cooperative_game<RealT> const& game, pid_type player)
{
	const ::std::size_t n(game.num_players());
	const RealT n_fact(::boost::math::factorial<RealT>(n));

	::std::vector<pid_type> players(game.players());
	::std::set<pid_type> other_players(players.begin(), players.end());
	players.clear();
	other_players.erase(player);

	RealT sv(0);

	if (other_players.size() > 0)
	{
		::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
		while (subset.has_next())
		{
			::std::size_t s(subset.size());
			RealT s_fact(::boost::math::factorial<RealT>(s));
			RealT nmsm1_fact(::boost::math::factorial<RealT>(n-s-1));

			::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
			cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
			tmp_players.push_back(player);
			cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

			sv += s_fact*nmsm1_fact*(game.value(sui_cid)-game.value(s_cid));

			++subset;
		}
	}
	else
	{
		cid_type cid = players_coalition<RealT>::make_id(&player, &player+1);
		sv += game.value(cid);
	}

	return sv/n_fact;
}

/**
 * \brief Compute the Shapley value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$, the Shapley value \f$\phi_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \phi_i(v)=\sum_{S \subseteq N \setminus \{i\}} \frac{|S|!\; (|N|-|S|-1)!}{|N|!}(v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
::std::map<pid_type,RealT> shapley_value(cooperative_game<RealT> const& game)
{
	const ::std::size_t n(game.num_players());
	const RealT n_fact(::boost::math::factorial<RealT>(n));

	::std::vector<pid_type> players(game.players());

	::std::map<pid_type,RealT> sv_map;

	::std::vector<pid_type>::const_iterator players_end_it(players.end());
	for (::std::vector<pid_type>::const_iterator players_it = players.begin();
		 players_it != players_end_it;
		 ++players_it)
	{
		pid_type pid(*players_it);

		RealT sv(0);

		::std::set<pid_type> other_players(players.begin(), players.end());
		other_players.erase(pid);

		if (other_players.size() > 0)
		{
			::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
			while (subset.has_next())
			{
				const ::std::size_t s(subset.size());
				const RealT s_fact(::boost::math::factorial<RealT>(s));
				const RealT nmsm1_fact(::boost::math::factorial<RealT>(n-s-1));

				::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
				cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
				tmp_players.push_back(pid);
				const cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

				sv += s_fact*nmsm1_fact*(game.value(sui_cid)-game.value(s_cid));

				++subset;
			}
		}
		else
		{
			cid_type cid = players_coalition<RealT>::make_id(&pid, &pid+1);
			sv += game.value(cid);
		}

		sv_map[pid] = sv/n_fact;
	}

	return sv_map;
}

/**
 * \brief Compute the Banzhaf value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$, the Banzhaf value \f$\beta_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \beta_i(v)=\frac{1}{2^{|N|-1}}\sum_{S \subseteq N \setminus \{i\}} (v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
::std::map<pid_type,RealT> banzhaf_value(cooperative_game<RealT> const& game)
{
	const ::std::size_t n(game.num_players());
	const RealT prod(1.0/::std::pow(2, n-1));
	//const RealT prod(1.0/(1 << (n-1)));

	::std::vector<pid_type> players(game.players());

	::std::map<pid_type,RealT> bv_map;

	::std::vector<pid_type>::const_iterator players_end_it(players.end());
	for (::std::vector<pid_type>::const_iterator players_it = players.begin();
		 players_it != players_end_it;
		 ++players_it)
	{
		pid_type player(*players_it);

		RealT bv(0);

		::std::set<pid_type> other_players(players.begin(), players.end());
		other_players.erase(player);

		if (other_players.size() > 0)
		{
			::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
			while (subset.has_next())
			{
				::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
				cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
				tmp_players.push_back(player);
				cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

				bv += (game.value(sui_cid)-game.value(s_cid));

				++subset;
			}
		}
		else
		{
			cid_type cid = players_coalition<RealT>::make_id(&player, &player+1);
			bv += game.value(cid);
		}

		bv_map[player] = bv*prod;
	}

	return bv_map;
}

/**
 * \brief Compute the normalized Banzhaf value for all the players of a given
 *  game.
 *
 * Given a game \f$(N,v)\f$, the normalized Banzhaf value \f$\bar{\beta}_i\f$
 * for player \f$i\f$ is computed as:
 * \f[
 *  \bar{\beta}_i=\frac{\beta_i}{\sum_{j \in N}\beta_j}
 * \f]
 * where
 * \f[
 *  \beta_i(v)=\frac{1}{2^{|N|-1}}\sum_{S \subseteq N \setminus \{i\}} (v(S\cup\{i\})-v(S))
 * \f]
 * is the non-normalized Banzhaf value.
 */
template <typename RealT>
::std::map<pid_type,RealT> norm_banzhaf_value(cooperative_game<RealT> const& game)
{
	::std::map<pid_type,RealT> bv_map = banzhaf_value(game);

	RealT den(0);
	// Compute the sum of all non-normalized Banzhaf values
	{
		typename ::std::map<pid_type,RealT>::const_iterator end_it(bv_map.end());
		for (typename ::std::map<pid_type,RealT>::const_iterator it = bv_map.begin();
			 it != end_it;
			 ++it)
		{
			den += it->second;
		}
	}
	// Normalize each Banzhaf value
	{
		::std::vector<pid_type> players(game.players());
		cid_type gc_cid = players_coalition<RealT>::make_id(players.begin(), players.end());
		RealT v_gc = game.value(gc_cid);
		typename ::std::map<pid_type,RealT>::iterator end_it(bv_map.end());
		for (typename ::std::map<pid_type,RealT>::iterator it = bv_map.begin();
			 it != end_it;
			 ++it)
		{
			it->second *= v_gc/den;
		}
//		typename ::std::map<pid_type,RealT>::iterator end_it(bv_map.end());
//		for (typename ::std::map<pid_type,RealT>::iterator it = bv_map.begin();
//			 it != end_it;
//			 ++it)
//		{
//			it->second /= den;
//		}
	}

	return bv_map;
}

/**
 * \brief Compute the Aumann-Dreze value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$ with coalition structure \f$\mathcal{P}\f$, the
 * Aumann-Dreze value \f$\text{AD}_i\f$ for player \f$i\f$ is computed as:
 * \f[
 *  \text{AD}_i(v)=\sum_{S \subseteq \mathcal{P}(i) \setminus \{i\}} \frac{|S|!\; (|\mathcal{P}(i)|-|S|-1)!}{|\mathcal{P}(i)|!}(v(S\cup\{i\})-v(S))
 * \f]
 * where \f$\mathcal{P}(i)\in\mathcal{P}\f$ is the coalition containing the
 * player $i$.
 *
 * References:
 * -# R.J. Aumann and J.H. Dr{\`e}ze,
 *    Cooperative games with coalition structures,
 *    International Journal of Game Theory 3:217-237, 1974
 * .
 */
template <typename RealT>
::std::map<pid_type,RealT> aumann_dreze_value(cooperative_game<RealT> const& game)
{
	const ::std::vector<cid_type> structure(game.coalition_structure());
	const ::std::size_t nc(structure.size());

	::std::map<pid_type,RealT> ad_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid(structure[i]);

		const ::std::vector<pid_type> players = game.coalition(cid).players();

		::std::map<pid_type,RealT> sh_map = shapley_value(game.subgame(cid));
		ad_map.insert(sh_map.begin(), sh_map.end());
	}

	return ad_map;
}

/**
 * \brief Compute the Chi-value for all the players of a given
 *  game.
 *
 * Given a game \f$(N,v)\f$ with coalition structure \f$\mathcal{P}\f$, the
 * Chi-value \f$\Chi_i\f$ for player \f$i\f$ is computed as:
 * \f[
 *  Chi_i=\phi_i(N,v)+\frac{v(\mathcal{P}(i)-\sum_{j\in\mathcal{P}(i)}\phi_j(N,v)}{|\mathcal{P}(i)|}
 * \f]
 * where\f$\mathcal{P}(i)\f$ is the coalition in \f$\mathcal{P}\f$ that contains
 * the player \f$i\f$.
 *
 * References:
 * -# Andre` Casajus. "Outside options, component efficiency, and stability", Games and Economic Behavior 65:49-61, 2009.
 * .
 */
template <typename RealT>
::std::map<pid_type,RealT> chi_value(cooperative_game<RealT> const& game)
{
	const ::std::vector<cid_type> structure(game.coalition_structure());
	const ::std::size_t nc(structure.size());
	const ::std::map<pid_type,RealT> shapley_map(shapley_value(game));

	::std::map< ::std::size_t,RealT> coal_shapley_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid = structure[i];
		const ::std::vector<pid_type> players = game.coalition(cid).players();
		const ::std::size_t np = players.size();

		RealT sh(0);
		for (::std::size_t j = 0; j < np; ++j)
		{
			const pid_type pid(players[j]);

			sh += shapley_map.at(pid);
		}
		coal_shapley_map[cid] = sh;
	}

	::std::map<pid_type,RealT> chi_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid = structure[i];
		const players_coalition<RealT> coalition = game.coalition(cid);
		const ::std::vector<pid_type> players = coalition.players();
		const ::std::size_t np = players.size();
		const RealT add = (coalition.value()-coal_shapley_map.at(cid))/np;

		for (::std::size_t j = 0; j < np; ++j)
		{
			const pid_type pid(players[j]);

			chi_map[pid] = shapley_map.at(pid)+add;
		}
	}

	return chi_map;
}

/**
 * \brief Compute the core of the given cooperative game.
 *
 * Given a cooperative game G=(N,v), the core C(v) is the set
 * \f[
 *  \biggl\bigl{x | \sum_{i \in N} x_i=v(N), \text{ and } x_i \ge v(i) \forall i \in N, \text{ and } \sum_{i\in S} x_i \ge v(S) \forall S \subset N\setminus \{\emptyset\bigr\}\biggr\}
 * \f]
 *
 * TODO: manage the case of games with a coalition structure CS. For such games, the core is defined as the set of payoff vectors x = (x1 , ..., xn ) and a coalition Structure CS = (C1 , ...,Ck ) such that:
 * - \forall C \subseteq N, x(C) \ge v(C) and
 * - x(C_j) = v(C_j) for any C_j \in CS
 * .
 */
template <typename RealT>
//core<RealT> find_core(typename coalition<RealT>::cid_type const& cid, ::std::vector<pid_type> const& players, ::std::map<cid_type,RealT> const& coalitions)
core<RealT> find_core(cooperative_game<RealT> const& game)
//typename coalition<RealT>::cid_type const& cid, ::std::vector<pid_type> const& players, ::std::map<cid_type,RealT> const& coalitions)
{
	typedef RealT real_type;
	typedef players_coalition<real_type> coalition_type;
	//typedef typename coalition_type::cid_type cid_type;
	typedef IloNumVarArray var_vector_type;
//	typedef IloArray<IloNumVarArray> var_matrix_type;
	typedef typename ::dcs::algorithm::lexicographic_subset subset_type;
	typedef typename subset_type::const_iterator subset_iterator;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container subset_container;
//	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container_const_iterator subset_container_iterator;

	core<real_type> kore;

	// Setting up vars
	try
	{
		// Initialize the Concert Technology app
		IloEnv env;

		IloModel model(env);

		model.setName("Core");

		::std::size_t n(game.num_players());
		::std::vector<pid_type> players(game.players());

		// Decision Variable

		// Variables x_{i} >= 0: the payoff for player i
		var_vector_type x(env, n);
		for (std::size_t i = 0; i < n; ++i)
		{
			std::ostringstream oss;
			oss << "x[" << i << "]";
			x[i] = IloNumVar(env, -IloInfinity, IloInfinity, ILOFLOAT, oss.str().c_str());
			model.add(x[i]);
		}

		// Constraints

		std::size_t cc(0); // Constraint counter
		std::size_t csc(0); // Constraint subcounter

		subset_type lex_subset(n, false);

		// C1: \forall S \subset N, \sum_{i \in S} x[i] >= v(S), and \sum_{i \in N} x[i] = v(N)
		++cc;
		while (lex_subset.has_next())
		{
			++csc;

			subset_container players_subset;

			//players_subset = ::dcs::algorithm::next_subset(players.begin(), players.end(), lex_subset);
			players_subset = lex_subset(players.begin(), players.end());

			cid_type cid = coalition_type::make_id(players_subset.begin(), players_subset.end());

			real_type v_cid = game.value(cid);

			std::ostringstream oss;
			oss << "C" << cc << "_{" << csc << "}";
			IloExpr lhs(env);
			subset_iterator sub_end_it(lex_subset.end());
			for (subset_iterator sub_it = lex_subset.begin();
				 sub_it != sub_end_it;
				 ++sub_it)
			{
				typename subset_iterator::value_type player_num(*sub_it);

				// check: player_num is a valid player number
				DCS_ASSERT(player_num < n,
						   DCS_EXCEPTION_THROW(::std::runtime_error, "Invalid player number"));

				lhs += x[player_num];
			}
			IloConstraint cons;
			if (players_subset.size() == n)
			{
				cons = IloConstraint(lhs == IloNum(v_cid));
			}
			else
			{
				cons = IloConstraint(lhs >= IloNum(v_cid));
			}
			cons.setName(oss.str().c_str());
			model.add(cons);

			++lex_subset;
		}

		// Set objective
		//   max z = \sum_{i=1}^N x_i
		IloObjective z;
		z = IloMaximize(env, IloSum(x));
		model.add(z);

		IloCplex solver(model);
#ifndef DCS_DEBUG
		solver.setOut(env.getNullStream());
		solver.setWarning(env.getNullStream());
#else // DCS_DEBUG
		solver.exportModel("cplex-core_model.lp");
#endif // DCS_DEBUG

		// Set Relative Gap to 1%: CPLEX will stop as soon as it has found a feasible integer solution proved to be within 1% of optimal.
		//if (relative_gap > 0)
		//{
		//	solver.setParam(IloCplex::EpGap, relative_gap);
		//}

		bool solved = solver.solve();

		IloAlgorithm::Status status = solver.getStatus();
		switch (status)
		{
			case IloAlgorithm::Optimal: // The algorithm found an optimal solution.
			case IloAlgorithm::Feasible: // The algorithm found a feasible solution, though it may not necessarily be optimal.
//
//				v = static_cast<RealT>(solver.getObjValue());
				break;
			case IloAlgorithm::Infeasible: // The algorithm proved the model infeasible (i.e., it is not possible to find an assignment of values to variables satisfying all the constraints in the model).
			case IloAlgorithm::Unbounded: // The algorithm proved the model unbounded.
			case IloAlgorithm::InfeasibleOrUnbounded: // The model is infeasible or unbounded.
			case IloAlgorithm::Error: // An error occurred and, on platforms that support exceptions, that an exception has been thrown.
			case IloAlgorithm::Unknown: // The algorithm has no information about the solution of the model.
			{
				::std::ostringstream oss;
				oss << "Optimization was stopped with status = " << status << " (CPLEX status = " << solver.getCplexStatus() << ", sub-status = " << solver.getCplexSubStatus() << ")";
				dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}

		if (solved)
		{
#ifdef DCS_DEBUG
			DCS_DEBUG_TRACE( "-------------------------------------------------------------------------------[" );
			DCS_DEBUG_TRACE( "- Objective value: " << static_cast<real_type>(solver.getObjValue()) );

			DCS_DEBUG_TRACE( "- Decision variables: " );

			// Output x_{i}
			for (std::size_t i = 0; i < n; ++i)
			{
				DCS_DEBUG_STREAM << x[i].getName() << " = " << solver.getValue(x[i]) << ::std::endl;
			}

			DCS_DEBUG_TRACE( "]-------------------------------------------------------------------------------" );
#endif // DCS_DEBUG

			::std::vector<real_type> payoff(n);
			for (std::size_t i = 0; i < n; ++i)
			{
				//payoff[players[i]] = solver.getValue(x[i]);
				payoff[i] = solver.getValue(x[i]);
			}
			kore = core<real_type>(payoff.begin(), payoff.end());
		}

		z.end();
		x.end();

		// Close the Concert Technology app
		env.end();
	}
	catch (IloException const& e)
	{
		::std::ostringstream oss;
		oss << "Got exception from CPLEX: " << e.getMessage();
		DCS_EXCEPTION_THROW(::std::runtime_error,
							oss.str());
	}
	catch (...)
	{
		DCS_EXCEPTION_THROW(::std::runtime_error,
							"Unexpected error during the optimization");
	}

	return kore;
}

/**
 * \brief Compute the core of the given cooperative game.
 *
 * Given a cooperative game G=(N,v), the core C(v) is the set
 * \f[
 *  \biggl\bigl{x | \sum_{i \in N} x_i=v(N), \text{ and } x_i \ge v(i) \forall i \in N, \text{ and } \sum_{i\in S} x_i \ge v(S) \forall S \subset N\setminus \{\emptyset\bigr\}\biggr\}
 * \f]
 */
template <typename RealT, typename IterT>
bool belongs_to_core(cooperative_game<RealT> const& game, IterT first_payoff, IterT last_payoff)
{
	typedef RealT real_type;
	typedef players_coalition<real_type> coalition_type;
	//typedef typename coalition_type::cid_type cid_type;
	typedef typename ::dcs::algorithm::lexicographic_subset subset_type;
	//typedef typename subset_type::const_iterator subset_iterator;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container subset_container;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container_const_iterator subset_container_iterator;

	::std::map<pid_type,real_type> x(first_payoff, last_payoff);
	::std::size_t n(game.num_players());
	::std::vector<pid_type> players(game.players());

	subset_type lex_subset(n, false);

	// \forall S \subset N, \sum_{i \in S} x[i] >= v(S), and \sum_{i \in N} x[i] = v(N)
	while (lex_subset.has_next())
	{
		subset_container players_subset;

		players_subset = lex_subset(players.begin(), players.end());

		cid_type cid = coalition_type::make_id(players_subset.begin(), players_subset.end());

		real_type v_cid = game.value(cid);
		real_type xs(0);

		subset_container_iterator sub_end_it(players_subset.end());
		for (subset_container_iterator sub_it = players_subset.begin();
			 sub_it != sub_end_it;
			 ++sub_it)
		{
			pid_type player_num(*sub_it);

			xs += x[player_num];
		}
		bool ok;
		if (lex_subset.size() == n)
		{
			//ok = xs == v_cid;
			ok = ::dcs::math::float_traits<real_type>::essentially_equal(xs, v_cid);
		}
		else
		{
			//ok = xs >= v_cid;
			ok = ::dcs::math::float_traits<real_type>::essentially_greater_equal(xs, v_cid);
		}
		if (!ok)
		{
			return false;
		}

		++lex_subset;
	}

	return true;
}

} // Namespace gtpack

#endif // GTPACK_COOPERATIVE_HPP


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
///
/// Code related to for simple nanostructures:
/// nanowires and nanoribbons.
/// This is done using approach developed by Li et. al.
/// see 10.1103/PhysRevB.85.195436 for theoretical
/// expressions. Scaling integrals are solved in
/// non-cartesian coordinates to make them easier
/// to work with

#include <unordered_map>
#include <constants.hpp>
#include <structures.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <isotopic_scattering.hpp>
#include <boost/mpi.hpp>
#if BOOST_VERSION >= 106700
#include <boost/container_hash/hash.hpp>
#else
#include <boost/functional/hash.hpp>
#endif
#include <Eigen/Dense>

/// Hash for pairs and arrays
namespace std {
template <typename T> struct hash<std::pair<T, T>> {
    std::size_t operator()(const std::pair<T, T>& key) const {
        hash<T> backend;
        std::size_t nruter = 0;
        boost::hash_combine(nruter, backend(key.first));
        boost::hash_combine(nruter, backend(key.second));
        return nruter;
    }
};
template <typename T, std::size_t S> struct hash<std::array<T, S>> {
    std::size_t operator()(const array<T, S>& key) const {
        hash<T> backend;
        std::size_t nruter = 0;

        for (auto& e : key)
            boost::hash_combine(nruter, backend(e));
        return nruter;
    }
};
} // namespace std

namespace alma {
namespace nanos {

/// Obtain the scaled lifetime for nanowires (see Eq. 8 of
/// 10.1103/PhysRevB.85.195436) the integral is
/// analitically solved in cilidrical coordinates
/// with z-axis being the transport axis
/// @param[in] tau0 - bulk lifetime
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] vel - phonon velocity
/// @param[in] R - nanowire radii
/// @return scaled tau
double scale_tau_nanowire(double tau0,
                          const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                          const Eigen::Ref<const Eigen::Vector3d>& vel,
                          double R);

/// Obtain the scaled lifetime for nanoribbons(see Eq. 8 of
/// 10.1103/PhysRevB.85.195436) the integral is
/// analitically solved in xy-rotated cartesian
/// coordinates with new y-axis (y') aligned with
/// transport axis. The nanoribbon is then centered
/// and defined from x' = [-L/2,L/2]
/// NOTE: the nanoribbon is contained in xy plane
///       otherwise it will fail.
/// @param[in] tau0 - bulk lifetime
/// @param[in] uaxis - transport axis (unitary vector)
/// @param[in] vel - phonon velocity
/// @param[in] L - nanoribbon width
/// @return scaled tau

double scale_tau_nanoribbon(double tau0,
                            const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                            const Eigen::Ref<const Eigen::Vector3d>& vel,
                            double L);

/// Obtain the thermal conductivity of a nanosystem
/// under RTA
///
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] w - RTA scattering rates
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] system_name - kind of nanosystem
/// @param[in] limiting_length - the limiting length of the nanostructure [nm]
/// @param[in] T - temperature in K
/// @return the RTA thermal conductivity over transpot axis
double calc_kappa_RTA(const alma::Crystal_structure& poscar,
                      const alma::Gamma_grid& grid,
                      const Eigen::Ref<const Eigen::ArrayXXd>& w,
                      const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                      const std::string& system_name,
                      const double limiting_length,
                      double T);

/// Obtain the thermal conductivity of a nanowires system
/// under the full BTE (beyond Relaxation Time Approximation).
/// This is achieved by deterministic solution a linear system
/// over full BZ due to symmetry breaking caused by boundaries
/// (i.e.: scaling of tau can be different for bulk equivalent
///    qpoints. Consequently we cannot use some symmetry operations)
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] syms - symmetry operations object
/// @param[in] emission_processes   - list of 3-phonon emission processes
/// @param[in] absorption_processes - list of 3-phonon  processes
/// @param[in] isotopic_processes   - list of 2-phonon processes
/// @param[in] w0 - RTA scattering rates
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] system_name - kind of nanosystem
/// @param[in] limiting_length - the limiting length of the nanostructure [nm]
/// @param[in] T - temperature in K
/// @param[in] iterative - use iterative Eigen solver for faster computation
/// @param[in] world - mpi communicator
/// @return the thermal conductivity over transpot axis
double calc_kappa_nanos(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    const Eigen::Ref<const Eigen::Vector3d>& uaxis,
    const std::string& system_name,
    const double limiting_length,
    double T,
    bool iterative,
    boost::mpi::communicator& world);

/// Get processes in full BZ
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] cell - description of the unit cell
/// @param[in] syms - symmetry operations object
/// @param[in,out] emission_processes   - list of 3-phonon emission processes
/// @param[in,out] absorption_processes - list of 3-phonon  processes
/// @param[in,out] isotopic_processes   - list of 2-phonon processes
/// @param[in] world - mpi communicator
/// @param[in] scalebroad_three - scale parameter for broadening in 3-ph
/// processes
void get_fullBZ_processes(
    const alma::Gamma_grid& grid,
    const alma::Crystal_structure& cell,
    std::string& anhIFCfile,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    boost::mpi::communicator& world,
    double scalebroad_three = 0.1);

} // namespace nanos
} // namespace alma


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// 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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP
#define CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP

#include <utility>
#include <vector>
#include <array>
#include <type_traits>
#include <iterator>
#include <algorithm>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <nil/crypto3/hash/algorithm/to_curve.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>

#include <nil/crypto3/detail/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace pubkey {
            namespace detail {
                template<typename policy_type>
                struct bls_basic_functions {
                    typedef typename policy_type::curve_type curve_type;
                    typedef typename policy_type::gt_value_type gt_value_type;
                    typedef typename policy_type::private_key_type private_key_type;
                    typedef typename policy_type::public_key_type public_key_type;
                    typedef typename policy_type::signature_type signature_type;
                    typedef typename policy_type::h2c_policy h2c_policy;

                    typedef typename policy_type::bls_serializer bls_serializer;
                    typedef typename policy_type::public_key_serialized_type public_key_serialized_type;
                    typedef typename policy_type::signature_serialized_type signature_serialized_type;

                    typedef typename policy_type::internal_accumulator_type internal_accumulator_type;
                    typedef std::pair<std::vector<public_key_type>, std::vector<internal_accumulator_type>>
                        internal_aggregation_accumulator_type;
                    typedef std::pair<std::vector<public_key_type>, internal_accumulator_type>
                        internal_fast_aggregation_accumulator_type;

                    constexpr static const std::size_t private_key_bits = policy_type::private_key_bits;
                    constexpr static const std::size_t L = static_cast<std::size_t>((3 * private_key_bits) / 16) +
                                                           static_cast<std::size_t>((3 * private_key_bits) % 16 != 0);
                    static_assert(L < 0x10000, "L is required to fit in 2 octets");
                    constexpr static const std::array<std::uint8_t, 2> L_os = {static_cast<std::uint8_t>(L >> 8u),
                                                                               static_cast<std::uint8_t>(L % 0x100)};

                    // TODO: implement key_gen
                    // template<typename IkmType, typename KeyInfoType>
                    // static inline private_key_type key_gen(const IkmType &ikm, const KeyInfoType &key_info) {}

                    static inline bool validate_private_key(const private_key_type &sk) {
                        return !sk.is_zero();
                    }

                    static inline public_key_type privkey_to_pubkey(const private_key_type &sk) {
                        BOOST_ASSERT(validate_private_key(sk));

                        return sk * public_key_type::one();
                    }

                    static inline bool validate_public_key(const public_key_type &pk) {
                        return !(pk.is_zero() || !pk.is_well_formed());
                    }

                    template<typename InputRange>
                    static inline void update(internal_accumulator_type &acc, const InputRange &range) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputRange>));

                        to_curve<h2c_policy>(range, acc);
                    }

                    template<typename InputIterator>
                    static inline void update(internal_accumulator_type &acc, InputIterator first, InputIterator last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));

                        to_curve<h2c_policy>(first, last, acc);
                    }

                    static inline signature_type sign(const internal_accumulator_type &acc,
                                                      const private_key_type &sk) {
                        BOOST_ASSERT(validate_private_key(sk));

                        signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(acc);
                        return sk * Q;
                    }

                    static inline bool verify(const internal_accumulator_type &acc, const public_key_type &pk,
                                              const signature_type &sig) {
                        /// check if signature point is on the curve
                        if (!sig.is_well_formed()) {
                            return false;
                        }
                        if (!validate_public_key(pk)) {
                            return false;
                        }
                        signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(acc);
                        auto C1 = policy_type::pairing(Q, pk);
                        auto C2 = policy_type::pairing(sig, public_key_type::one());
                        return C1 == C2;
                    }

                    template<
                        typename SignatureIterator,
                        typename = typename std::enable_if<std::is_same<
                            signature_type, typename std::iterator_traits<SignatureIterator>::value_type>::value>::type>
                    static inline void aggregate(signature_type &acc, SignatureIterator sig_first,
                                                 SignatureIterator sig_last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<SignatureIterator>));
                        assert(std::distance(sig_first, sig_last) > 0);

                        while (sig_first != sig_last) {
                            signature_type next_p = *sig_first++;
                            acc = acc + next_p;
                        }
                    }

                    template<typename SignatureRange,
                             typename = typename std::enable_if<std::is_same<
                                 signature_type, typename std::iterator_traits<
                                                     typename SignatureRange::iterator>::value_type>::value>::type>
                    static inline void aggregate(signature_type &acc, const SignatureRange &sig_n) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<SignatureRange>));

                        aggregate(acc, std::cbegin(sig_n), std::cend(sig_n));
                    }

                    static inline bool aggregate_verify(const internal_aggregation_accumulator_type &acc,
                                                        const signature_type &sig) {
                        const typename internal_aggregation_accumulator_type::first_type &pk_n = acc.first;
                        const typename internal_aggregation_accumulator_type::second_type &acc_n = acc.second;
                        assert(std::distance(pk_n.begin(), pk_n.end()) > 0 &&
                               std::distance(pk_n.begin(), pk_n.end()) == std::distance(acc_n.begin(), acc_n.end()));

                        if (!sig.is_well_formed()) {
                            return false;
                        }
                        auto pk_n_iter = std::cbegin(pk_n);
                        auto acc_n_iter = std::cbegin(acc_n);
                        gt_value_type C1 = gt_value_type::one();
                        while (pk_n_iter != std::cend(pk_n) && acc_n_iter != std::cend(acc_n)) {
                            if (!validate_public_key(*pk_n_iter)) {
                                return false;
                            }
                            signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(*acc_n_iter++);
                            C1 = C1 * policy_type::pairing(Q, *pk_n_iter++);
                        }
                        return C1 == policy_type::pairing(sig, public_key_type::one());
                    }

                    static inline bool aggregate_verify(const internal_fast_aggregation_accumulator_type &acc,
                                                        const signature_type &sig) {
                        const typename internal_fast_aggregation_accumulator_type::first_type &pk_n = acc.first;
                        const typename internal_fast_aggregation_accumulator_type::second_type &msg_acc = acc.second;
                        assert(std::distance(pk_n.begin(), pk_n.end()) > 0);

                        auto pk_n_iter = pk_n.begin();
                        public_key_type aggregate_p = *pk_n_iter++;
                        while (pk_n_iter != pk_n.end()) {
                            public_key_type next_p = *pk_n_iter++;
                            aggregate_p = aggregate_p + next_p;
                        }
                        return verify(msg_acc, aggregate_p, sig);
                    }

                    static inline signature_type pop_prove(const private_key_type &sk) {
                        assert(validate_private_key(sk));

                        public_key_type pk = privkey_to_pubkey(sk);
                        signature_type Q = to_curve<h2c_policy>(point_to_pubkey(pk));
                        return sk * Q;
                    }

                    static inline bool pop_verify(const public_key_type &pk, const signature_type &pop) {
                        if (!pop.is_well_formed()) {
                            return false;
                        }
                        if (!validate_public_key(pk)) {
                            return false;
                        }
                        signature_type Q = to_curve<h2c_policy>(point_to_pubkey(pk));
                        auto C1 = policy_type::pairing(Q, pk);
                        auto C2 = policy_type::pairing(pop, public_key_type::one());
                        return C1 == C2;
                    }

                    static inline public_key_serialized_type point_to_pubkey(const public_key_type &pk) {
                        return bls_serializer::point_to_octets_compress(pk);
                    }

                    static inline signature_serialized_type point_to_signature(const signature_type &sig) {
                        return bls_serializer::point_to_octets_compress(sig);
                    }
                };
            }    // namespace detail
        }        // namespace pubkey
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP


#include <rovi_pose_estimator/rovi_pose_estimator.h>

#include <iostream>
#include <vector>
#include <tuple>

#include <ros/ros.h>
#include <ros/package.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Int32.h>

#include <Eigen/Eigen>
#include <eigen_conversions/eigen_msg.h>

#include <opencv4/opencv2/imgproc.hpp>
#include <opencv4/opencv2/core.hpp>
#include <opencv4/opencv2/imgcodecs.hpp>
#include <opencv4/opencv2/highgui.hpp>
#include <opencv4/opencv2/flann.hpp>
#include <opencv4/opencv2/calib3d.hpp>
#include <opencv4/opencv2/stereo.hpp>
#include <opencv4/opencv2/ximgproc/disparity_filter.hpp>
#include <opencv4/opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>

#include <pcl/features/normal_3d.h>
#include <pcl/features/spin_image.h>

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

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/icp.h>

// #include <pcl/surface/mls.h>
// #include <pcl/surface/impl/mls.hpp>
// #include <pcl/surface/bilateral_upsampling.h>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

#include <pcl/common/random.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>

#include <rovi_gazebo/rovi_gazebo.h>
#include <rovi_utils/rovi_utils.h>

namespace rovi_pose_estimator
{

// Scene
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_scene_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_orig_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filtered_scene_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::Histogram<153>>::Ptr features_scene_ptr(new pcl::PointCloud<pcl::Histogram<153>>());

// Object
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filtered_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::Histogram<153>>::Ptr features_object_ptr(new pcl::PointCloud<pcl::Histogram<153>>());

// Correspondences
static pcl::Correspondences corr;

std::array<cv::Mat, 2>
M1::get_image_data(const std::string & ns_ros)
{
    static std::mutex mutex_image_l;
    static sensor_msgs::Image image_l;
    static auto thread_left = rovi_utils::create_async_listener(ns_ros + "/left/image_raw", image_l, mutex_image_l);

    static std::mutex mutex_image_r;
    static sensor_msgs::Image image_r;
    static auto thread_right = rovi_utils::create_async_listener(ns_ros + "/right/image_raw", image_r, mutex_image_r);

    static bool first = true;

    std::array<cv::Mat, 2> arr;
    cv::Mat temp_l, temp_r;

    std::lock_guard<std::mutex> lock_l(mutex_image_l);
    std::lock_guard<std::mutex> lock_r(mutex_image_r);

    auto ptr_image_l = boost::make_shared<const sensor_msgs::Image>(image_l);
    auto ptr_image_r = boost::make_shared<const sensor_msgs::Image>(image_r);

    cv_bridge::toCvShare(ptr_image_l, "bgr8")->image.copyTo(temp_l);
    arr[0] = temp_l;
    cv_bridge::toCvShare(ptr_image_r, "bgr8")->image.copyTo(temp_r);
    arr[1] = temp_r;

    return arr;
}

void
M1::set_structed_light(ros::NodeHandle & nh, const bool & state)
{
    static auto handler = nh.advertise<std_msgs::Int32>("/projector_controller/projector", 1);

    // spam the buffer
    ros::Rate lp(200);
    auto tic = ros::Time::now();
    while (1.0 > (ros::Time::now() - tic).toSec())
    {
        // ROS_INFO_STREAM((ros::Time::now() - tic).toSec());
        std_msgs::Int32 msg;
        msg.data = ( state == 1 ) ? 1 : 0;
        handler.publish(msg);
        lp.sleep();
    }
}

std::array<sensor_msgs::CameraInfo, 2>
M1::get_image_info(const std::string & ns_ros)
{
    std::array<sensor_msgs::CameraInfo, 2> arr;

    for (const auto & [idx, cam] : std::array{std::tuple(0, std::string("/left/camera_info")), std::tuple(1, std::string("/right/camera_info"))})
    {
        // get the message
        auto msg = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(ns_ros + cam);
        arr[idx] = *msg;
    }
    return arr;
}

cv::Mat
M1::compute_disparitymap(const cv::Mat & img_left, const cv::Mat & img_right)
{
    // FileStorage
    cv::FileStorage fs("stereosgbm_config.yaml", cv::FileStorage::READ);

    static auto min_disp          = 0;
    static auto num_disp          = 16*8;
    static auto block_size        = 5; // has to be odd
    static auto cn                = 3;
    static auto p1_smoothness     = 100;
    static auto p2_smoothness     = 400; // he larger the values are, the smoother the disparity is
    static auto disp_12_max       = 1;
    static auto unique_ratio      = 5;
    static auto speckle_win_size  = 0;
    static auto specke_range      = 1;
    static auto filter_gap        = 2;
    static auto written           = 0;
    static auto lambda            = 2;
    static auto sigma             = 5;

    // // Check if the file exists
    fs["written"] >> written;
    // ROS_INFO_STREAM_ONCE("Checking if the file exists stereosgbm_config.yaml exists, the state is: " << written);
    // ROS_INFO_STREAM_ONCE("If not, write to the file");

    if (fs.isOpened() && written == 1)
    {
        //read from file
        fs["min_disp"]         >> min_disp;
        fs["num_disp"]         >> num_disp;
        fs["block_size"]       >> block_size;
        fs["cn"]               >> cn;
        fs["p1_smoothness"]    >> p1_smoothness;
        fs["p2_smoothness"]    >> p2_smoothness;
        fs["disp_12_max"]      >> disp_12_max;
        fs["unique_ratio"]     >> unique_ratio;
        fs["speckle_win_size"] >> speckle_win_size;
        fs["specke_range"]     >> specke_range;
        fs["filter_gap"]       >> filter_gap;
        fs["lambda"]           >> lambda;
        fs["sigma"]            >> sigma;
    }
    else
    {
        // write to file
        cv::FileStorage fs1("stereosgbm_config.yaml", cv::FileStorage::WRITE);
        fs1 << "written"          << 1;
        fs1 << "min_disp"         << min_disp;
        fs1 << "num_disp"         << num_disp;
        fs1 << "block_size"       << block_size;
        fs1 << "cn"               << cn;
        fs1 << "p1_smoothness"    << p1_smoothness;
        fs1 << "p2_smoothness"    << p2_smoothness;
        fs1 << "disp_12_max"      << disp_12_max;
        fs1 << "unique_ratio"     << unique_ratio;
        fs1 << "speckle_win_size" << speckle_win_size;
        fs1 << "specke_range"     << specke_range;
        fs1 << "filter_gap"       << filter_gap;
        fs1 << "sigma"            << sigma;
        fs1 << "lambda"           << lambda;
    }

    // Write the configurations
    ROS_INFO_STREAM( min_disp           << ", " <<
                     num_disp           << ", " <<
                     block_size         << ", " <<
                     cn                 << ", " <<
                     p1_smoothness      << ", " <<
                     p2_smoothness      << ", " <<
                     disp_12_max        << ", " <<
                     unique_ratio       << ", " <<
                     speckle_win_size   << ", " <<
                     specke_range       << ", " <<
                     filter_gap
                    );

    // Setup the matchers
    cv::Ptr<cv::StereoSGBM> left_matcher = cv::StereoSGBM::create(
                                                                min_disp,
                                                                num_disp,
                                                                block_size,
                                                                p1_smoothness,
                                                                p2_smoothness,
                                                                disp_12_max,
                                                                filter_gap,
                                                                unique_ratio,
                                                                speckle_win_size,
                                                                specke_range,
                                                                cv::StereoSGBM::MODE_SGBM_3WAY
                                                              );

    // Use the recomended solvers from OpenCV
    cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls = cv::ximgproc::createDisparityWLSFilter(left_matcher);
    cv::Ptr<cv::StereoMatcher> right_matcher = cv::ximgproc::createRightMatcher(left_matcher);

    // For plotting the sets, define 5 cv::Mat
    cv::Mat disp_left, disp_right, disp_filtered, img_left_temp, img_right_temp, point_cloud;

    // Color convert
    cv::cvtColor(img_left,  img_left_temp, cv::COLOR_BGR2GRAY);
    cv::cvtColor(img_right, img_right_temp, cv::COLOR_BGR2GRAY);

    // Remove noise before canny, always.
    cv::GaussianBlur(img_left_temp, img_left_temp, GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);
    cv::GaussianBlur(img_right_temp, img_right_temp, GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);

    // Compute left and right disparity
    left_matcher->compute(img_left_temp, img_right_temp, disp_left);
    right_matcher->compute(img_right_temp, img_left_temp, disp_right);

    // Computed weighted least squares optimization
    wls->setLambda(lambda);
    wls->setSigmaColor(sigma);
    wls->filter(disp_left, img_left, disp_filtered, disp_right);

    // Compute Disparity Map
    cv::ximgproc::getDisparityVis(disp_filtered, disp_filtered, 1.0);
    // cv::imwrite("disparity_map.jpg", disp_filtered);

    // Define the Camera matrix
    static auto cam_info_arr = M1::get_image_info();
    static Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_left(cam_info_arr[LEFT].K.data());

    // Compute the Q-matrix
    static cv::Mat Q = (cv::Mat_<double>(4, 4)<< 1.f, 0.f,  0.f, -K_left(0,2),
                                                 0.f, 1.f,  0.f, -K_left(1,2),
                                                 0.f, 0.f,  0.f,  K_left(1,1),
                                                 0.f, 0.f, -1.f/BASELINE, 0.f );

    // Reproject the image to 3D
    cv::reprojectImageTo3D(disp_filtered, point_cloud, Q, false);

    return point_cloud;
};

void
M1::compute_pointcloud_scene(const cv::Mat & point_cloud, const cv::Mat & left_img)
{

    // cloud_ptr set width etc
    cloud_scene_ptr->width = point_cloud.cols;
    cloud_scene_ptr->height = point_cloud.rows;
    cloud_scene_ptr->is_dense = false;
    cloud_scene_ptr->resize(cloud_scene_ptr->width * cloud_scene_ptr->height);

    // these are reuqired for the for loop
    for (int i = 0, m = 0, k = 0; i < point_cloud.rows; ++i)
    {
        const float* point_cloud_ele = point_cloud.ptr<float>(i);
        const uchar* rbg_left = left_img.ptr<uchar>(i);
        m = 0;

        for(int j = 0; j < point_cloud.cols * 3; )
        {

            std::uint8_t b = (std::uint8_t) rbg_left[m++];
            std::uint8_t g = (std::uint8_t) rbg_left[m++];
            std::uint8_t r = (std::uint8_t) rbg_left[m++];

            cloud_scene_ptr -> points[k].x = point_cloud_ele[j++];
            cloud_scene_ptr -> points[k].y = point_cloud_ele[j++];
            cloud_scene_ptr -> points[k].z = point_cloud_ele[j++];

            std::uint32_t rgb = ( (uint32_t) r << 24 | (uint32_t) g << 16 | (uint32_t) b << 8);

            cloud_scene_ptr -> points[k++].rgb = *reinterpret_cast<float*>(&rgb);
        }
    }

    // Compute the transformation matrix from world to gazebo camera
    static Eigen::Affine3d w_T_gaze;
    tf::poseMsgToEigen(rovi_gazebo::get_model_pose("camera_stereo"), w_T_gaze);

    // Get constant OPENGL transformation
    static Eigen::Matrix4f gaze_T_c  = ( Eigen::Matrix4f() << 0.f, 0.f, 1.f, 0.f,
                                                             -1.f, 0.f, 0.f, 0.f,
                                                              0.f,-1.f, 0.f, 0.f,
                                                              0.f, 0.f, 0.f, 1.f ).finished();

    // Define the overall transformation
    static Eigen::Matrix4f trans = (w_T_gaze.cast<float>()).matrix() * gaze_T_c;

    // Transform the point cloud
    pcl::transformPointCloud(*cloud_scene_ptr, *cloud_scene_ptr, trans);

    // Cropbox
    static pcl::CropBox<pcl::PointXYZRGBNormal> box_filter;
    {
        box_filter.setMin(Eigen::Vector4f(0.0, 0.85, 0.70, 1.0f));
        box_filter.setMax(Eigen::Vector4f(0.8, 1.25, 1.25, 1.0f));
        box_filter.setInputCloud(cloud_scene_ptr);
        box_filter.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
        //*cloud_orig_ptr = *cloud_scene_ptr;
    }

    // Plane fitting
    constexpr auto dist_tsh = 0.01f;
    static pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
    static pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    static pcl::SACSegmentation<pcl::PointXYZRGBNormal> segm;
    {
        segm.setOptimizeCoefficients(true);
        segm.setModelType(pcl::SACMODEL_PLANE);
        segm.setMethodType(pcl::SAC_RANSAC);
        segm.setDistanceThreshold(dist_tsh);
        segm.setInputCloud(cloud_scene_ptr);
        segm.segment(*inliers, *coeff);

        ROS_INFO_STREAM("Number of inliers for plane_segmentation: " << inliers->indices.size());

        if (inliers->indices.size() != 0 )
        {
            double zmin = 0;
            for (const auto &idx : inliers->indices)
                zmin += cloud_scene_ptr->points[idx].z;
            zmin /= inliers->indices.size();

            // Create filtering object
            pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
            pass.setInputCloud(cloud_scene_ptr);
            pass.setFilterFieldName("z");
            pass.setFilterLimits(zmin + 0.025, zmin + 1);
            pass.setFilterLimitsNegative(false);
            pass.filter(*filtered_scene_ptr);
            *cloud_scene_ptr = *filtered_scene_ptr;
        }
    }

    // Voxel Filter
    static pcl::VoxelGrid<pcl::PointXYZRGBNormal> voxel_filter;
    {
        voxel_filter.setInputCloud(cloud_scene_ptr);
        voxel_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
        voxel_filter.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
    }

    // Statistical outlier removal
    static pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBNormal> sor;
    {
        sor.setInputCloud(cloud_scene_ptr);
        sor.setMeanK(10);
        sor.setStddevMulThresh(1);
        sor.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
    }

    // Estimate normals from PoV
    static pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> normal_est;
    {
        pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
        normal_est.setInputCloud(cloud_scene_ptr);
        normal_est.setViewPoint(trans(0,3), trans(1,3), trans(2,3));
        normal_est.setSearchMethod(tree);
        normal_est.setKSearch(25);
        normal_est.compute(*cloud_scene_ptr);
    }

    static pcl::SpinImageEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Histogram<153>> spin;
    {
        spin.setInputCloud(cloud_scene_ptr);
        spin.setInputNormals(cloud_scene_ptr);
        spin.setRadiusSearch(0.15);
        spin.compute(*features_scene_ptr);
    }

    // Save the pointcloud
    // pcl::io::savePCDFileASCII("scene_voxel.pcd", *cloud_scene_ptr);
}

bool
M1::read_compute_features_object(const std::string & obj)
{

    int error = pcl::io::loadPCDFile<pcl::PointXYZRGBNormal>(obj, *cloud_object_ptr);

    if(error != 0)
    {
        return false;
    }

    // Compute the transformation matrix from world to gazebo camera
    static Eigen::Affine3d w_T_gaze;
    tf::poseMsgToEigen(rovi_gazebo::get_model_pose("camera_stereo"), w_T_gaze);

    // Get constant OPENGL transformation
    static Eigen::Matrix4f gaze_T_c  = ( Eigen::Matrix4f() << 0.f, 0.f, 1.f, 0.f,
                                                             -1.f, 0.f, 0.f, 0.f,
                                                              0.f,-1.f, 0.f, 0.f,
                                                              0.f, 0.f, 0.f, 1.f ).finished();
    // Define the overall transformation
    static Eigen::Matrix4f trans = (w_T_gaze.cast<float>()).matrix() * gaze_T_c;

    // Give some arbitrary colours to the object
    for (size_t i = 0; i < cloud_object_ptr->size(); i++)
    {
        std::uint8_t b = (std::uint8_t) 200;
        std::uint8_t g = (std::uint8_t) 150;
        std::uint8_t r = (std::uint8_t) 128;
        std::uint32_t rgb = ( (uint32_t) r << 24 | (uint32_t) g << 16 | (uint32_t) b << 8);
        cloud_object_ptr -> points[i].rgb = *reinterpret_cast<float*>(&rgb);
    }

    // Voxel Filter
    static pcl::VoxelGrid<pcl::PointXYZRGBNormal> voxel_filter;
    {
        voxel_filter.setInputCloud(cloud_object_ptr);
        voxel_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
        voxel_filter.filter(*filtered_object_ptr);
        *cloud_object_ptr = *filtered_object_ptr;
    }

    // Estimate normals from PoV
    static pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> normal_est;
    {
        pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
        normal_est.setInputCloud(cloud_object_ptr);
        normal_est.setViewPoint(trans(0,3), trans(1,3), trans(2,3));
        normal_est.setSearchMethod(tree);
        normal_est.setKSearch(25);
        normal_est.compute(*cloud_object_ptr);
    }

    static pcl::SpinImageEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Histogram<153>> spin;
    {
        spin.setInputCloud(cloud_object_ptr);
        spin.setInputNormals(cloud_object_ptr);
        spin.setRadiusSearch(0.15);
        spin.compute(*features_object_ptr);
    }

    // Save the pointcloud
    //pcl::io::savePCDFileASCII("voxel_object.pcd", *cloud_object_ptr);

    return true;
}

void
M1::match_features()
{

    // Define the L2_norm operator
    static auto L2_norm = [](const pcl::Histogram<153> & f1, const pcl::Histogram<153> & f2)
    {
        float sum = 0.f;
        for(auto i = 0; i < f1.descriptorSize(); i++)
            sum += std::pow((f1.histogram[i]-f2.histogram[i]), 2);
        return sum;
    };

    // Define nearest neightbour
    static auto nearest_neighbour = [](const pcl::PointCloud<pcl::Histogram<153>>::Ptr & query, const int idx_query, const pcl::PointCloud<pcl::Histogram<153>>::Ptr & scene, int & idx_scene)
    {
        float min_dist = std::numeric_limits<float>::max();

        for(auto i = 0; i < scene->size(); i++)
        {
            float dist = L2_norm(query->points[idx_query], scene->points[i]);
            if (min_dist > dist)
            {
                min_dist = dist;
                idx_scene = i;
            }
        }

        return min_dist;
    };

    // Correspondences are reset.
    corr.clear();
    corr.resize(features_object_ptr->size());

    // Start matching
    int idx_scene = 0;
    for(auto idx_query = 0; idx_query < features_object_ptr->size(); idx_query++)
    {
        corr[idx_query].index_query    = idx_query;
        corr[idx_query].distance       = nearest_neighbour(features_object_ptr, idx_query, features_scene_ptr, idx_scene);
        corr[idx_query].index_match    = idx_scene;
    }
}

Eigen::Matrix4f
M1::ransac_features(const int & max_it)
{
    // Note
    ROS_INFO_STREAM("Amount of iterations: " << max_it);

    // Metrics
    int max_inliers = 0;
    Eigen::Matrix4f best_T, T;

    // Variables
    std::vector<int> query(3), scene(3);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr aligned_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());

    // Random generator
    pcl::common::UniformGenerator<int> rnd_gen(0, corr.size() - 1);
    rnd_gen.setSeed(time(0));

    // Define search tree
    pcl::KdTreeFLANN<pcl::PointXYZRGBNormal> kd_tree_scene;
    kd_tree_scene.setInputCloud(cloud_scene_ptr);

    for(auto i = 0; i < max_it; i++)
    {
        // How far are we?
        if(i % 500 == 0)
        {
            ROS_INFO_STREAM("Iteration: " << i);
        }

        // Pick 3 points
        for (auto j = 0; j < 3; j++)
        {
            int random = rnd_gen.run();
            query[j] = corr[random].index_query;
            scene[j] = corr[random].index_match;
        }
        // Determine the transformation between cloud and cloud_object, we use the object and the scene
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> svd;
        svd.estimateRigidTransformation(*cloud_object_ptr, query, *cloud_scene_ptr, scene, T);

        // Perform the transformation on the two clouds, here we usedaligned
        pcl::transformPointCloud(*cloud_object_ptr, *aligned_object_ptr, T);

        // Perform NN
        int K = 1;
        std::vector<int> point_idx_nn_search(K);
        std::vector<float> point_nn_L2(K);

        // Inlier
        int inliers = 0;

        for (auto j = 0; j < aligned_object_ptr->size(); j++)
        {
            if ( kd_tree_scene.nearestKSearch(aligned_object_ptr->points[j], K, point_idx_nn_search, point_nn_L2) > 0 )
            {
                if (point_nn_L2[0] < 0.00001f)
                {
                    inliers++;
                }
            }
        }

        if (inliers > max_inliers)
        {
            ROS_INFO_STREAM("Inliers: " << inliers);
            max_inliers = inliers;
            best_T = T;
        }
    }

    pcl::transformPointCloud(*cloud_object_ptr, *cloud_object_ptr, best_T);

    // pcl::visualization::PCLVisualizer viewer("vis");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_orig_ptr, "scene");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_object_ptr, "obj");
    // viewer.spin();

    pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal > icp;
    icp.setInputSource(cloud_object_ptr);
    icp.setInputTarget(cloud_scene_ptr);
    icp.setRANSACOutlierRejectionThreshold(leaf_size*1.2);
    icp.setMaxCorrespondenceDistance(0.005);
    icp.setEuclideanFitnessEpsilon(1e-9);
    icp.setTransformationEpsilon(1e-9);
    icp.setMaximumIterations(20);
    icp.align(*cloud_object_ptr);

    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_orig_ptr, "scene");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_object_ptr, "obj1");
    // viewer.spin();

    return icp.getFinalTransformation() * best_T; //;

}

geometry_msgs::Pose
M1::estimate_pose(const int & it, const bool & draw, const double & noise)
{
    static auto file_bottle_pcd = ros::package::getPath("rovi_gazebo") + std::string("/models/bottle/bottle.pcd");

    // Get stereo images
    auto cam_images = M1::get_image_data();

    cv::Mat gaussian_noise_left = cv::Mat::zeros(cam_images[0].size(), cam_images[0].type());
    cv::Mat gaussian_noise_right = gaussian_noise_left;

    std::vector<double> mean = { 0, 0, 0 };
    std::vector<double> std = { noise, noise, noise };

    cv::randn(gaussian_noise_left, mean, std);
    cv::randn(gaussian_noise_right, mean, std);

    if(draw)
    {
        cv::imwrite("left.jpg", gaussian_noise_left);
    }

    cam_images[0] += gaussian_noise_left;
    cam_images[1] += gaussian_noise_right;

    // Compute the disparity map
    cv::Mat point_cloud = M1::compute_disparitymap(cam_images[LEFT], cam_images[RIGHT]);

    if(draw)
    {
        cv::imwrite("bottle_left.jpg",  cam_images[0]);
        cv::imwrite("bottle_right.jpg", cam_images[1]);
        std::ofstream write_cloud("write_mat.mat", std::ios_base::out);
        write_cloud << cv::format(point_cloud, cv::Formatter::FMT_CSV) << std::endl;
        write_cloud.close();
    }

    M1::compute_pointcloud_scene(point_cloud, cam_images[0]);
    M1::read_compute_features_object(file_bottle_pcd);
    M1::match_features();

    auto T = Eigen::Affine3d(M1::ransac_features(it).cast<double>());
    geometry_msgs::Pose pose;
    tf::poseEigenToMsg(T, pose);

    return pose;
}

}


// Copyright 2017 Reinaldo Astudillo and Martin van Gijzen
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// 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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// IDR(s) for solving systems of linear equations, biorthogonal version.
//
// Reference:
// Martin B. van Gijzen and Peter Sonneveld, Algorithm 913: An Elegant IDR(s)
// Variant that Efficiently Exploits Bi-orthogonality Properties.
// ACM Transactions on Mathematical Software, Vol. 38, No. 1, pp. 5:1-5:19, 2011

#ifndef ITL_BIDR_S_INCLUDE
#define ITL_BIDR_S_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/random.hpp>
#include <boost/numeric/mtl/operation/orth.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl {
using namespace mtl;

/// Induced Dimension Reduction (IDR(s))
template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	   class Preconditioner, class Iteration >
int idr_s(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	     const Preconditioner& M, Iteration& iter, size_t s)
{
    typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
    typedef HilbertSpaceX                                       Vector;
    mtl::vampir_trace<7010> tracer;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");
    if (s < 1) s= 1;

    const Scalar                zero= math::zero(Scalar());
    const Scalar                one= math::one(Scalar());
    const double angle = 0.7;
    Vector                      v(resource(x)), t(resource(x)), r(b - A * x);
    mat::multi_vector<Vector>   G(Vector(resource(x), zero), s), U(Vector(resource(x), zero), s), P(Vector(resource(x), zero), s);
    dense_vector<Scalar>   f(s), c(s);
    mat::dense2D<Scalar>        Ms(s, s); Ms = one;
    double om = 1.0;
    random(P);
    orth(P);
    // Main iteration loop, build G-spaces:
    while (! iter.finished(r)) {
         // New righ-hand size for small system:
         f = trans(P) * r;
         for (size_t k= 0; k < s; k++) {
            // Solve small system and make v orthogonal to P
            irange range(k, s);
            c[range] = lower_trisolve(Ms[range][range], f[range]);
            // Preconditioning
            v = solve(M, r - G.vector(range)*c[range]);
            // Compute new U(:,k) and G(:,k), G(:,k) is in space G_j
            U.vector(k) = U.vector(range)*c[range] + om*v;
            // Matvec
            G.vector(k) = A*U.vector(k);
            // Bi-Orthogonalise the new basis vectors
            for (size_t i= 0; i < k; i++) {
                Scalar alpha = dot(P.vector(i), G.vector(k))/Ms[i][i];
                G.vector(k) = G.vector(k) - alpha*G.vector(i);
                U.vector(k) = U.vector(k) - alpha*U.vector(i);
            }
            // New column of M = P'*G  (first k-1 entries are zero)
            for (size_t i= k; i < s; i++) {
                Ms[i][k] = dot(P.vector(i), G.vector(k));
            }
            // Make r orthogonal to g_i, i = 1..k
            Scalar beta = f[k] / Ms[k][k];
            r = r - beta*G.vector(k);
            x = x + beta*U.vector(k);
            if ((++iter).finished(r))
                return iter;
            if (k < s-1) {
               for (size_t i= k+1; i < s; i++) {
                   f[i] = f[i] - beta*Ms[i][k];
               }
            }
         }
         // Now we have sufficient vectors in G_j to compute residual in G_j+1
         // Note: r is already perpendicular to P so v = r
         // Preconditioning
         v = solve(M, r);
         // Matvec
         t = A*v;
         // Computation of a new omega
         double ns = two_norm(r);
         double nt = two_norm(t);
         double ts = dot(t, r);
         double rho = fabs(ts/(nt*ns));
         om=ts/(nt*nt);
         if (rho < angle)
             om = om*angle/rho;
         r = r - om*t;
         x = x + om*v;
         ++iter;
    }
    return iter;
}

} // namespace itl

#endif // ITL_BIDR_S_INCLUDE


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_PROD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/two_split.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/fusion/tuple.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      typedef typename meta::as_logical<A0>::type bA0;
      r0 = a*b;
      A0 a1, a2, b1, b2;
      bA0 isinf = logical_and(logical_or(is_inf(b), is_inf(a)), is_inf(r0));
      two_split(a, a1, a2);
      two_split(b, b1, b2);
      r1 = if_zero_else(isinf, a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2));
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_prod(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                           (A0)(X),
                           ((simd_<floating_<A0>,X>))
                           ((simd_<floating_<A0>,X>))
                          )
  {
    typedef typename boost::fusion::tuple<A0,A0> result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_prod(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }
#endif