/*  $Id: voronoidiagram.cpp 664 2011-02-13 17:37:33Z anders.e.e.wallin $
 *
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef VODI_G_H
#define VODI_G_H

#include <vector>

#include <boost/graph/adjacency_list.hpp>

#include "point.h"
#include "halfedgediagram.hpp"

namespace ocl {

// this file contains typedefs used by voronoidiagram.h

struct VertexProps;
struct EdgeProps;
struct FaceProps;

typedef unsigned int HEFace;

// the type of graph with which we construct the voronoi-diagram
typedef HEDIGraph<     boost::listS,             // out-edges stored in a std::list
                       boost::listS,             // vertex set stored here
                       boost::bidirectionalS,    // bidirectional graph.
                       VertexProps,              // vertex properties
                       EdgeProps,                // edge properties
                       FaceProps,                // face properties
                       boost::no_property,       // graph properties
                       boost::listS              // edge storage
                       > HEGraph;

typedef boost::graph_traits< HEGraph >::vertex_descriptor  HEVertex;
typedef boost::graph_traits< HEGraph >::vertex_iterator    HEVertexItr;
typedef boost::graph_traits< HEGraph >::edge_descriptor    HEEdge;
typedef boost::graph_traits< HEGraph >::edge_iterator      HEEdgeItr;
typedef boost::graph_traits< HEGraph >::out_edge_iterator  HEOutEdgeItr;
typedef boost::graph_traits< HEGraph >::adjacency_iterator HEAdjacencyItr;
typedef boost::graph_traits< HEGraph >::vertices_size_type HEVertexSize;

// typedef std::vector< std::vector< HEEdge > > HEPlanarEmbedding;

/// voronoi-vertices can be of these four different types
/// as we incrementally construct the diagram the type is updated as follows:
/// OUT-vertices will not be deleted
/// IN-vertices will be deleted
/// UNDECIDED-vertices have not been examied yet
/// NEW-vertices are constructed on OUT-IN edges
enum VoronoiVertexType {OUT, IN, UNDECIDED, NEW };

/// properties of a vertex in the voronoi diagram
struct VertexProps {
    VertexProps() {
        init();
    }
    /// construct vertex at position p with type t
    VertexProps( Point p, VoronoiVertexType t) {
        position=p;
        type=t;
        init();
    }
    void init() {
        index = count;
        count++;
        in_queue = false;
    }
    void reset() {
        in_queue = false;
        type = UNDECIDED;
    }
    /// based on previously calculated J2, J3, and J4, set the position of the vertex
    void set_position() {
        double w = J4;
        double x = - J2/w+pk.x;
        double y = J3/w+pk.y;
        position =  Point(x,y);
    }
    /// based on precalculated J2, J3, J4, calculate the H determinant for Point pl
    double detH(const Point& pl) {
        H = J2*(pl.x-pk.x) - J3*(pl.y-pk.y) + 0.5*J4*(square(pl.x-pk.x) + square(pl.y-pk.y));
        return H;
    }
    bool operator<(const VertexProps& other) const {
        return ( abs(this->H) < abs(other.H) );
    }
    /// set the J values
    void set_J(const Point& pi, const Point& pj, const Point& pkin) {
        // 1) i-j-k should come in CCW order
        Point pi_,pj_,pk_;
        if ( pi.isRight(pj,pkin) ) {
            pi_ = pj;
            pj_ = pi;
            pk_ = pkin;
        } else {
            pi_ = pi;
            pj_ = pj;
            pk_ = pkin;
        }
        assert( !pi_.isRight(pj_,pk_) );
        // 2) point pk should have the largest angle
        // largest angle is opposite longest side.
        Point pi__,pj__,pk__;
        pi__ = pi_;
        pj__ = pj_;
        pk__ = pk_;
        double longest_side = (pi_ - pj_).xyNorm();
        if (  (pj_ - pk_).xyNorm() > longest_side ) {
            longest_side = (pj_ - pk_).xyNorm(); //j-k is longest, so i should be new k
            pk__ = pi_;                         // old  i-j-k
            pi__ = pj_;                         // new  k-i-j
            pj__ = pk_;
        }
        if ( (pi_ - pk_).xyNorm() > longest_side ) { // i-k is longest, so j should be new k
            pk__ = pj_;                          // old  i-j-k
            pj__ = pi_;                          // new  j-k-i
            pi__ = pk_;
        }

        assert( !pi__.isRight(pj__,pk__) );
        assert( (pi__ - pj__).xyNorm() >=  (pj__ - pk__).xyNorm() );
        assert( (pi__ - pj__).xyNorm() >=  (pk__ - pi__).xyNorm() );

        this->pk = pk__;
        J2 = detH_J2( pi__, pj__, pk__);
        J3 = detH_J3( pi__, pj__, pk__);
        J4 = detH_J4( pi__, pj__, pk__);
        //assert( J4 > 0.0 );
    }
    /// calculate J2
    double detH_J2(Point& pi, Point& pj, Point& pk) {
        return (pi.y-pk.y)*(square(pj.x-pk.x)+square(pj.y-pk.y))/2 - (pj.y-pk.y)*(square(pi.x-pk.x)+square(pi.y-pk.y))/2;
    }
    /// calculate J3
    double detH_J3(Point& pi, Point& pj, Point& pk) {
        return (pi.x-pk.x)*(square(pj.x-pk.x)+square(pj.y-pk.y))/2 - (pj.x-pk.x)*(square(pi.x-pk.x)+square(pi.y-pk.y))/2;
    }
    /// calculate J4
    double detH_J4(Point& pi, Point& pj, Point& pk) {
        return (pi.x-pk.x)*(pj.y-pk.y) - (pj.x-pk.x)*(pi.y-pk.y);
    }
// VD DATA
    /// vertex type
    VoronoiVertexType type;
    /// the reference point for J-calculations
    Point pk;
    /// J2 determinant
    double J2;
    /// J3 determinant
    double J3;
    /// J4 determinant
    double J4;
    double H;
    bool in_queue;
// HE data
    /// the position of the vertex
    Point position;
    /// index of vertex
    int index;
    /// global vertex count
    static int count;
};

/// properties of an edge in the voronoi diagram
/// each edge stores a pointer to the next HEEdge
/// and the HEFace to which this HEEdge belongs
struct EdgeProps {
    EdgeProps() {}
    /// create edge with given next, twin, and face
    EdgeProps(HEEdge n, HEEdge t, HEFace f){
        next = n;
        twin = t;
        face = f;
    }
    /// the next edge, counterclockwise, from this edge
    HEEdge next;
    /// the twin edge
    HEEdge twin;
    /// the face to which this edge belongs
    HEFace face;
};

/// types of faces in the voronoi diagram
enum VoronoiFaceType {INCIDENT, NONINCIDENT};

/// properties of a face in the voronoi diagram
/// each face stores one edge on the boundary of the face
struct FaceProps {
    /// create face with given edge, generator, and type
    FaceProps( HEEdge e , Point gen, VoronoiFaceType t) {
        edge = e;
        generator = gen;
        type = t;
    }
    /// operator for sorting faces
    bool operator<(const FaceProps& f) const {return (this->idx<f.idx);}
    /// face index
    HEFace idx;
    /// one edge that bounds this face
    HEEdge edge;
    /// the generator for this face
    Point generator;
    /// face type
    VoronoiFaceType type;
};

// these containers are used instead of iterators when accessing
// adjacent vertices, edges, faces.
// FIXME: it may be faster to rewrite the code so it uses iterators, as does the BGL.
typedef std::vector<HEVertex> VertexVector;
typedef std::vector<HEFace> FaceVector;
typedef std::vector<HEEdge> EdgeVector;

}
#endif


// Copyright (C) 2007-2012 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Nicholas Edmonds
//           Douglas Gregor
//           Andrew Lumsdaine

/**************************************************************************
 * This source file implements the Delta-stepping algorithm:              *
 *                                                                        *
 *   Ulrich Meyer and Peter Sanders. Parallel Shortest Path for Arbitrary *
 *   Graphs. In Proceedings from the 6th International Euro-Par           *
 *   Conference on Parallel Processing, pages 461--470, 2000.             *
 *                                                                        *
 *   Ulrich Meyer, Peter Sanders: [Delta]-stepping: A Parallelizable      *
 *   Shortest Path Algorithm. J. Algorithms 49(1): 114-152, 2003.         *
 *                                                                        *
 * There are several potential optimizations we could still perform for   *
 * this algorithm implementation:                                         *
 *                                                                        *
 *   - Implement "shortcuts", which bound the number of reinsertions      *
 *     in a single bucket (to one). The computation of shortcuts looks    *
 *     expensive in a distributed-memory setting, but it could be         *
 *     ammortized over many queries.                                      *
 *                                                                        *
 *   - The size of the "buckets" data structure can be bounded to         *
 *     max_edge_weight/delta buckets, if we treat the buckets as a        *
 *     circular array.                                                    *
 *                                                                        *
 *   - If we partition light/heavy edges ahead of time, we could improve  *
 *     relaxation performance by only iterating over the right portion    *
 *     of the out-edge list each time.                                    *
 *                                                                        *
 *   - Implement a more sophisticated algorithm for guessing "delta",     *
 *     based on the shortcut-finding algorithm.                           *
 **************************************************************************/
#ifndef BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <am++/counter_coalesced_message_type.hpp>
#include <am++/detail/thread_support.hpp>

#include <boost/parallel/append_buffer.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/parallel/algorithm.hpp> // for all_reduce
#include <boost/graph/parallel/thread_support.hpp> // for compare_and_swap
#include <algorithm> // for std::min, std::max
#include <boost/format.hpp>
#include <iostream>
#include <atomic>

template <typename OwnerMap, typename VertexDistanceData>
struct vertex_distance_owner {
  explicit vertex_distance_owner(const OwnerMap& owner) : owner(owner) {}

  const OwnerMap& owner;
};

template <typename OwnerMap, typename VertexDistanceData>
typename boost::property_traits<OwnerMap>::value_type
get(const vertex_distance_owner<OwnerMap, VertexDistanceData>& o, const VertexDistanceData& data)
{ return get(o.owner, data.first); }

namespace boost { namespace graph { namespace distributed {

template<typename Graph,
	 typename DistanceMap,
	 typename EdgeWeightMap,
	 typename WorkStats,
         typename Bucket = append_buffer<typename graph_traits<Graph>::vertex_descriptor, 10u>,
         typename MessageGenerator =
           amplusplus::simple_generator<amplusplus::counter_coalesced_message_type_gen> >
class delta_stepping_shortest_paths {
  typedef delta_stepping_shortest_paths<Graph, DistanceMap, EdgeWeightMap, Bucket>
    self_type;

  typedef typename boost::property_map<Graph, vertex_owner_t>::const_type OwnerMap;

  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::degree_size_type Degree;
  typedef typename property_traits<EdgeWeightMap>::value_type Dist;

  typedef typename std::vector<Bucket*>::size_type BucketIndex;

  typedef std::pair<Vertex, Dist> vertex_distance_data;

  struct vertex_distance_handler;

  typedef typename MessageGenerator::template call_result<vertex_distance_data, vertex_distance_handler,
				     vertex_distance_owner<OwnerMap, vertex_distance_data>,
				     amplusplus::idempotent_combination_t<boost::parallel::minimum<Dist>,
									  Dist> >::type
    RelaxMessage;

public:

  delta_stepping_shortest_paths(Graph& g,
                                DistanceMap& distance,
                                EdgeWeightMap& weight,
				amplusplus::transport &t,
                                Dist delta,
				int offs,
				WorkStats& stats,
                                MessageGenerator message_gen =
                                  MessageGenerator(amplusplus::counter_coalesced_message_type_gen(1 << 12)))
    : dummy_first_member_for_init_order((amplusplus::register_mpi_datatype<vertex_distance_data>(), 0)),
      g(g), transport(t), distance(distance), weight(weight), delta(delta),
    core_offset(offs),
    work_stats(stats),
    owner(get(vertex_owner, g)),
      level_sync(false), current_level(0), buckets_processed(0), current_bucket(0),
      relax_msg(message_gen, transport, vertex_distance_owner<OwnerMap, vertex_distance_data>(owner),
		amplusplus::idempotent_combination(boost::parallel::minimum<Dist>(), std::numeric_limits<Dist>::max()))
  {
    initialize();
  }

#ifdef AMPLUSPLUS_PRINT_HIT_RATES
  typedef std::pair<unsigned long long, unsigned long long> cache_stats;
  cache_stats
  get_cache_stats() {
    cache_stats stats(0, 0);
    for(size_t i = 0; i < relax_msg.counters.hits.size(); ++i) {
      stats.first += relax_msg.counters.hits[i];
      stats.second += relax_msg.counters.tests[i];
    }
    return stats;
  }
#endif // AMPLUSPLUS_PRINT_HIT_RATES

  void set_source(Vertex s) { source = s; } // for threaded execution
  void set_level_sync() { level_sync = true; } // force level-synchronized exploration
  void operator() (int tid) { run(source, tid); }

  void run(Vertex s, int tid = 0);

  BucketIndex get_num_levels() { return buckets_processed; }
  time_type get_start_time() { return start_time; }

protected:
  void initialize();

  // Relax the edge (u, v), creating a new best path of distance x.
  void relax(Vertex v, Dist x);

  void find_next_bucket();

  const int dummy_first_member_for_init_order; // Unused

  const Graph& g;
  amplusplus::transport& transport;
  DistanceMap& distance;
  EdgeWeightMap& weight;
  Dist delta;
  int core_offset;
  WorkStats& work_stats;
  OwnerMap owner;
  Vertex source;
  bool level_sync;

  // Bucket data structure. The ith bucket contains all local vertices
  // with (tentative) distance in the range [i*delta,
  // (i+1)*delta).
  std::vector<shared_ptr<Bucket> > buckets;

  // Bucket to hold vertices deleted at each level
  shared_ptr<Bucket> deleted_vertices;
  BucketIndex current_level; // How many buckets have we processed?
  BucketIndex buckets_processed; // Stats tracking
  BucketIndex num_buckets;

  // Shared thread state to make sure we're all on the same page
  BucketIndex current_bucket;
  shared_ptr<amplusplus::detail::barrier> t_bar;
  RelaxMessage relax_msg;
  time_type start_time;
};

#define DELTA_STEPPING_SHORTEST_PATHS_PARMS                                   \
      typename Graph, typename DistanceMap, typename EdgeWeightMap, typename WorkStats, typename Bucket, typename MessageGenerator

#define DELTA_STEPPING_SHORTEST_PATHS_TYPE                                    \
      delta_stepping_shortest_paths<Graph, DistanceMap, EdgeWeightMap, WorkStats, Bucket, MessageGenerator>

template<DELTA_STEPPING_SHORTEST_PATHS_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_TYPE::initialize()
{

  relax_msg.set_handler(vertex_distance_handler(*this));

  // Setup distance map
  distance.set_consistency_model(0);
  set_property_map_role(vertex_distance, distance);

  using boost::parallel::all_reduce;
  using boost::parallel::maximum;
  using std::max;

  // Compute the maximum edge weight
  Dist max_edge_weight = 0;
  Dist local_max = 0;
  // CULPRIT !
  BGL_FORALL_VERTICES_T(u, g, Graph) {
    BGL_FORALL_OUTEDGES_T(u, e, g, Graph) {
      auto w = get(weight, e);
      if (local_max < w)
	local_max = w;
    }
  }

  // do an all reduce to find the maximum
  MPI_Allreduce(&local_max, &max_edge_weight, 1,
		MPI_UINT32_T, MPI_MAX, MPI_COMM_WORLD);

  //  all_reduce<Dist, maximum<Dist> > r(transport, maximum<Dist>());
  //max_edge_weight = r(max_edge_weight);

  // If delta wasn't supplied in the ctor initialize it
  if (delta == 0) {

    // Compute the maximum edge degree
    Degree max_degree = 0;
    BGL_FORALL_VERTICES_T(u, g, Graph) {
      max_degree = max BOOST_PREVENT_MACRO_SUBSTITUTION (max_degree, out_degree(u, g));
    }
    // max_degree = all_reduce(process_group(g), max_degree, maximum<Degree>());
    all_reduce<Degree, maximum<Degree> > r(transport, maximum<Degree>());
    max_degree = r(max_degree);

    // Take a guess at delta, based on what works well for random
    // graphs.
    delta = max_edge_weight / max_degree;
    if (delta == 0)
      delta = 1;
  }

  //
  // Initialize buckets data structure
  //

  // Extra bucket is so we don't try to insert into the bucket we're processing
  // when the index wraps
  num_buckets = max_edge_weight / delta;
  num_buckets += (num_buckets * delta < (BucketIndex)max_edge_weight) ? 2 : 1;

  // Declare bucket data structure and index variable
  buckets.resize(num_buckets);
  for (BucketIndex i = 0 ; i < buckets.size() ; ++i) {
    shared_ptr<Bucket> p(new Bucket);
    buckets[i].swap(p);
  }

  shared_ptr<Bucket> p(new Bucket);
  deleted_vertices.swap(p);

  // Initialize distance labels
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    put(distance, v, (std::numeric_limits<Dist>::max)());
  }
}

template<DELTA_STEPPING_SHORTEST_PATHS_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_TYPE::run(Vertex s, int tid)
{
  /**
   *  NOTE: We never remove a vertex from a higher bucket when
   *  placing it in a lower one because thread-safe insertion is
   *  cheap as long as we don't have to handle removal
   *  simultaneously, this will result in additional work and higher
   *  memory consumption, but the alternative is locking which is
   *  sure to be slow.
   */
  int count_epoch = 0 ;
  AMPLUSPLUS_WITH_THREAD_ID(tid) {
    int nthreads = transport.get_nthreads();

    if (tid == 0)
      t_bar.reset(new amplusplus::detail::barrier(nthreads));

    // This barrier acts as a temporary barrier until we can be sure t_bar is initialized
    { amplusplus::scoped_epoch epoch(transport); }
    // Now above if branch needs to be executed to every thread
    // Therefore wait till every thread comes to this point
    t_bar->wait();

    // if two processes are running on the same node, core_offset
    // is important to achieve thread affinity
    if (pin(tid+core_offset) != 0) {
      std::cerr << "[ERROR] Unable to pin current thread to "
		<< "core : " << tid << std::endl;
      assert(false);
    }

    // wait till all threads are pinned
    t_bar->wait();
    { amplusplus::scoped_epoch epoch(transport); }

    validate_thread_core_relation();

    // last passing thread will update the correct start time
    start_time = get_time();
    // Push the source onto the queue
    {
      amplusplus::scoped_epoch epoch(transport);

      if (get(owner, s) == transport.rank() && tid == 0)
        relax(s, 0);
    }

    t_bar->wait();

    typename Bucket::size_type current_bucket_start, current_bucket_end;

    do {

      current_bucket_start = 0;
      current_bucket_end = buckets[current_bucket]->size();

      // Process current bucket
      do {

        unsigned long all_starting_sizes;
        const unsigned long starting_size = current_bucket_end - current_bucket_start;
        count_epoch = 0;
        t_bar->wait();
        {
          amplusplus::scoped_epoch_value epoch(transport, starting_size, all_starting_sizes);

#ifdef PRINT_DEBUG
          if (tid == 0)
            std::cerr << transport.rank() << ": processing light edges in bucket "
                      << current_bucket << " [" << current_bucket_start << ", " << current_bucket_end << ")\n";
#endif

	  while (current_bucket_start != current_bucket_end) {

	    for (typename Bucket::size_type i = current_bucket_start + tid ; i < current_bucket_end ; i+= nthreads) {
	      Vertex v = (*buckets[current_bucket])[i];
	      Dist dv = distance[v];

              // This is an optimization that is not specified in the algorithm
	      if (dv < delta * (Dist)current_level) {
		continue;
              }

	      // Add v to set of deleted vertices
	      deleted_vertices->push_back(v);

	      // Relax light edges
	      BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
		Vertex u = target(e, g);
		Dist we = get(weight, e);
		if (we <= delta) {// light edge
		  relax_msg.send(vertex_distance_data(u, dv + we));
#ifdef PRINT_DEBUG
                  std::cerr << "  Relax " << get(get(vertex_local, g), v) << "@"
                            << get(get(vertex_owner, g), v) << "->"
                            << get(get(vertex_local, g), u) << "@"
                            << get(get(vertex_owner, g), u) << "  weight = "
                            << we << std::endl;
#endif
                }
              }
            }

            // Wait for all threads to finish current bucket
            t_bar->wait();
            current_bucket_start = current_bucket_end;
            current_bucket_end = buckets[current_bucket]->size();
            t_bar->wait();

	    if (level_sync) break;
	  } // No more re-insertions into this bucket to process
	}
	count_epoch += 1;
	// If all processes are done with the current bucket:
	//   1. clear the current bucket
	//   2. process heavy edges
	//   3. find the next bucket to work on
	if (all_starting_sizes == 0) {

	  if (tid == 0) buckets[current_bucket]->clear();
	  t_bar->wait();

	  // Process heavy edges
	  // TODO: If we had a separate transport here we could do single-level termination detection
	  {
	    amplusplus::scoped_epoch epoch(transport);

	    int nthreads = transport.get_nthreads();
	    for (typename Bucket::size_type i = tid ; i < deleted_vertices->size() ; i += nthreads) {
	      Vertex v = (*deleted_vertices)[i];
	      Dist dv = distance[v];

	      BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
		Vertex u = target(e, g);
		Dist we = get(weight, e);
		if (we > delta) // heavy edge
		  relax_msg.send(vertex_distance_data(u, dv + we));
	      }
	    }
	  } // end_epoch does a thread barrier

#ifdef PRINT_DEBUG
          std::cerr << tid << "@" << transport.rank() << ": Current bucket " << current_bucket
                    << " size " << buckets[current_bucket]->size() << std::endl;
#endif
          assert(buckets[current_bucket]->size() == 0);

          if (tid == 0) deleted_vertices->clear();

          // find next bucket with work and update current_level
          BucketIndex old_bucket = current_bucket;

          t_bar->wait();
          if (tid == 0) {
            find_next_bucket();

            if (current_bucket != (std::numeric_limits<BucketIndex>::max)()) {
              current_level += current_bucket - old_bucket;
              ++buckets_processed;
            }
          }
          t_bar->wait();

          break; // exit the do..while loop for the current bucket
        } else // Update the end of the bucket and continue processing it
          current_bucket_end = buckets[current_bucket]->size();

      } while(true);

      // If there are no non-empty buckets in any process, we're done
    } while(current_bucket != (std::numeric_limits<BucketIndex>::max)());
  }
}

template<DELTA_STEPPING_SHORTEST_PATHS_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_TYPE::relax(Vertex v, Dist d)
{
  using boost::parallel::val_compare_and_swap;

  Dist old_dist = distance[v], last_old_dist;
  while (d < old_dist) {
    last_old_dist = old_dist;
    old_dist = val_compare_and_swap(&distance[v], old_dist, d);
    if (last_old_dist == old_dist) {
#ifdef PBGL2_PRINT_WORK_STATS
      int tid = amplusplus::detail::get_thread_id();
      if(old_dist < std::numeric_limits<Dist>::max()) {
	work_stats.increment_invalidated(tid);
      } else {
	work_stats.increment_useful(tid);
      }
#endif
      // Insert vertex into new bucket, note we don't remove it from any other
      // buckets it might be in
      BucketIndex new_index =
        static_cast<BucketIndex>((d - (current_level * delta)) / delta);

      //
      // If new_index == 0, relax all light edges and add it to deleted_vertices
      //
      if (new_index == 0) { // This vertex is bound for the current bucket
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	  Vertex u = target(e, g);
	  Dist we = get(weight, e);
	  if (we <= delta) // light edge
	    relax_msg.send(vertex_distance_data(u, d + we));
	}

	deleted_vertices->push_back(v);

        break; // No need to insert into current bucket now
      }

      new_index = (current_bucket + new_index) % num_buckets;
      buckets[new_index]->push_back(v);

      // new_distance now == get(distance, v) so we exit automatically
      // but this saves us the conditional test
      break;
    }
  }
#ifdef PBGL2_PRINT_WORK_STATS
  work_stats.increment_rejected(amplusplus::detail::get_thread_id());
#endif
  return;
}

template<DELTA_STEPPING_SHORTEST_PATHS_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_TYPE::find_next_bucket()
{
  using boost::parallel::all_reduce;
  using boost::parallel::minimum;

  BucketIndex old_bucket = current_bucket;
  BucketIndex max_bucket = (std::numeric_limits<BucketIndex>::max)();

  current_bucket = (current_bucket + 1) % buckets.size();
  while (current_bucket != old_bucket && buckets[current_bucket]->empty())
    current_bucket = (current_bucket + 1) % buckets.size();

  if (current_bucket == old_bucket)
    current_bucket = max_bucket;

  // If we wrapped, project index past end of buckets to use min()
  if (current_bucket < old_bucket) current_bucket += buckets.size();

  all_reduce<BucketIndex, minimum<BucketIndex> > r(transport, minimum<BucketIndex>());
  current_bucket = r(current_bucket);

  // Map index back into range of buckets
  if (current_bucket != max_bucket)
    current_bucket %= buckets.size();
}

template<DELTA_STEPPING_SHORTEST_PATHS_PARMS>
struct DELTA_STEPPING_SHORTEST_PATHS_TYPE::
vertex_distance_handler {

  vertex_distance_handler() : self(NULL) {}
  vertex_distance_handler(delta_stepping_shortest_paths& self) : self(&self) {}

  void operator() (const vertex_distance_data& data) const {
    self->work_stats.increment_edges(amplusplus::detail::get_thread_id());

    if (data.second < get(self->distance, data.first))
      self->relax(data.first, data.second);
#ifdef PBGL2_PRINT_WORK_STATS
    else
      self->work_stats.increment_rejected(amplusplus::detail::get_thread_id());
#endif
  }

protected:
  delta_stepping_shortest_paths* self;
};

} } } // end namespace boost::graph::distributed

#endif // BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "lnn_optimization.hpp"

#include <boost/format.hpp>

#include <core/functor.hpp>
#include <core/utils/timer.hpp>

#include <reversible/circuit.hpp>
#include <reversible/functions/copy_circuit.hpp>
#include <reversible/functions/copy_metadata.hpp>
#include <reversible/functions/add_gates.hpp>

#include <stack>
#include <climits>

namespace cirkit
{

/**
 * optimizationmode
 */
  enum lnn_optimization_mode {
    LNN_OPTIMIZATION_NONE,
    LNN_OPTIMIZATION_NAIVE,
    LNN_OPTIMIZATION_LOCAL_REORDER,
    LNN_OPTIMIZATION_GLOBAL_REORDER,
  };

  /**
     calculates the nearest neighbor cost between two lines, where line1 != line2
  */
  inline unsigned nnc_func(unsigned u1, unsigned u2){
    return u1 < u2 ? u2-u1-1 : u1-u2-1;
  }

/**
 * @brief Function providing the mapping of a specific new allocated line to its index
 *
 * @param src Source line which should be looked up
 * @param allocation Array representing the lines and their current position
 * @param length Length of the allocation array
 *
 * @return The line index of the source line in the allocation
 *
 */
  unsigned lookup(unsigned src, unsigned allocation[], unsigned length)
  {
    for(unsigned index = 0; index < length; index++)
      if(allocation[index] == src)
        return index;
    return 0;
  }

/**
 * @brief Function providing the mapping of two specific new allocated lines to their indices
 *
 * @param src1 First source line which should be looked up
 * @param src2 Second source line which should be looked up
 * @param allocation Array representing the lines and their current position
 * @param length Length of the allocation array
 *
 * @return the indices of two source lines in the allocation
 *
 */
  std::pair<unsigned,unsigned> lookup2(unsigned src1, unsigned src2, unsigned allocation[], unsigned length)
  {
    unsigned i1 = 0;
    unsigned i2 = 0;
    for(unsigned index = 0; index < length; index++){
        if(allocation[index] == src1)
    i1 = index;
        if(allocation[index] == src2)
    i2 = index;
    }
    return std::make_pair(i1,i2);
  }

  /**
   * @brief Function calculating the nearest neighbor cost (nnc) of a gate wrt. a line permutation (allocation)
   *
   * @param g Specific gate for which the nnc should be calculated
   * @param allocation Array representing the lines and their current position
   * @param len Length of the allocation array
   *
   * @return the nnc of the gate
   */
  unsigned nnc(const gate& g, unsigned *allocation, unsigned len){
    if(g.size() == 2 && g.controls().front().line() < len && g.targets().front() < len ){
      auto ctrltar = lookup2(g.controls().front().line(),g.targets().front(), allocation, len);
      return nnc_func(ctrltar.first, ctrltar.second);
    }
    return 0;
  }

  /**
   * @brief Function calculating the nearest neighbor cost (nnc) of a gate
   *
   * @param g Specific gate for which the nnc should be calculated
   * @return the nnc of the gate
   */
  unsigned nnc(const gate& g){
    if(g.size() == 2)
      return nnc_func(g.controls().front().line(), g.targets().front() );
    return 0;
  }

  /**
   * @brief Function calculating the nearest neighbor cost (nnc) of a circuit, which is the sum of the nnc of all gates
   *
   * @param circ Specific circuit for which the nnc should be calculated
   * @return the nnc of the circuit
   */
  unsigned NNC(const circuit& circ){
    unsigned sum = 0;
    for ( const gate& g : circ )
      sum += nnc(g);
    return sum;
  }

/**
 * @brief Function appending a swap gate exchanging line l1 and line l2 on circuit
 *
 * @param circ Circuit to which the swap gate should be appended
 * @param line1 first target line of the fredkin gate
 * @param line2 second target line of the fredkin gate
 */
  inline void append_swap_gate(circuit& circ, unsigned line1, unsigned line2)
  {
    gate::control_container lc; //control line container is empty
    append_fredkin(circ, lc , line1, line2);
  }

  /**
   * @brief sets the (first) control line of the gate
   *
   * @param source_gate The specific gate to modify
   * @param newcontrolline The new control line of the gate
   * @return A copy of the gate where the (first) control line is changed to the specified one
   */
  gate set_gate_control_line(const gate source_gate, unsigned newcontrolline){
    gate gate = source_gate;
    gate.remove_control(gate.controls().front());
    gate.add_control(make_var(newcontrolline));
    return gate;
  }

  /**
   * @brief sets the (first) target line of the gate
   *
   * @param source_gate The specific gate to modify
   * @param newtargetline The new target line of the gate
   * @return A copy of the gate where the (first) target line is changed to the specified one
   */
  gate set_gate_target_line(const gate source_gate, unsigned newtargetline){
    gate gate = source_gate;
    gate.remove_target(gate.targets().front());
    gate.add_target(newtargetline);
    return gate;
  }

/**
 * @brief applies the optimization of the naive lnn_optimization_mode on a circuit.
 *
 * @param circ Target circuit of the performing action
 * @param base Source circuit providing metadata and gates
 *
 * @return the nnc of the circuit
 */
  unsigned apply_naive_scheme(circuit& circ, const circuit& base){
    copy_metadata(base,circ);
    std::stack<std::pair<unsigned,unsigned> > stack;

    for ( const gate& g : base ){
      unsigned nnc_val = nnc(g);
      if(nnc_val>0){
        bool direction = g.controls().front().line() > g.targets().front() ;
        unsigned s = g.controls().front().line() ;

        for(unsigned i = 0; i < nnc_val; i++){

          auto swap = direction ?
            std::make_pair(s - i , s - 1 -i) :
            std::make_pair(s + i , s + 1 +i) ;
          append_swap_gate(circ , swap.first, swap.second);
          stack.push(swap);
        }

        unsigned newcontrol = g.targets().front() + (direction ? 1 : -1);
        circ.append_gate() = set_gate_control_line(g, newcontrol);

        for(; !stack.empty(); stack.pop())
          append_swap_gate(circ,stack.top().first,stack.top().second);
      }
      else
        circ.append_gate() = g;
    }

    return 2*NNC(base);
  }

  /**
   * @brief Function applies a permutation on a gate permuting its control and target line
   *
   * @param src_gate The gate to transform
   * @param allocation Array representing the lines and their current position
   * @return the transformed gate
   */
  inline gate transform_gate(const gate src_gate, unsigned* allocation){
    gate transformedgate;
    transformedgate.set_type(src_gate.type());
    transformedgate.add_target(allocation[src_gate.targets().front()]);
    if(src_gate.size() > 1){
      transformedgate.add_control(make_var(allocation[src_gate.controls().front().line()]));
    }
    return transformedgate;
  }

  /**
   * @brief Function applies a permutation on a gate permuting its control and target line
   *
   * @param src_gate The gate to transform
   * @param allocation Array representing the lines and their current position
   * @param len the length of the allocation array
   * @return the transformed gate
   */
  inline gate transform_gate2(const gate src_gate, unsigned* allocation, unsigned len){
    gate transformedgate;
    transformedgate.set_type(src_gate.type());

    if(src_gate.size() == 1){
      auto tar = lookup(src_gate.targets().front(), allocation, len);
      transformedgate.add_target(tar);
    }else{
      auto ctrltar = lookup2(src_gate.controls().front().line(),src_gate.targets().front(), allocation, len);
      transformedgate.add_control(make_var(ctrltar.first));
      transformedgate.add_target(ctrltar.second);
    }
    return transformedgate;
  }

  /**
   * @brief Function creates and initiates an allocation array
   *
   * @return the allocation array
   */
  unsigned* init_allocation(unsigned lines){
    unsigned *allocation;
    allocation = new unsigned[lines];
    for(unsigned i=0; i<lines; ++i)
      allocation[i] = i;
    return allocation;
  }

/**
 * @brief After switching lines in other functions and changing the allocation, this function maps the allocation on the output and garbage strings
 *
 * @param circ Target circuit of the performing action
 * @param allocation Array representing the lines and their current position
 * @param length Length of the allocation array
 *
 * @return Returns true if the inputs have the right length and the action can be performed
 */
  bool permute_outputs_and_garbage_lines(circuit& circ, unsigned* allocation, unsigned length){
    if(circ.lines() != length)
      return false;

    std::vector<std::string> out = circ.outputs();
    std::vector<std::string> new_out( circ.lines() );
    std::vector<bool> garbage = circ.garbage();
    std::vector<bool> new_garbage( circ.lines() );

    for ( unsigned u = 0; u<length; u++ ){
      new_out[u] = out[allocation[u]];
      new_garbage[u] = garbage[allocation[u]];
    }

    circ.set_outputs(new_out);
    circ.set_garbage(new_garbage);

    return true;
}

/**
 * @brief After switching lines in other functions and changing the allocation, this function maps the allocation on the input and constant strings
 *
 * @param circ Target circuit of the performing action
 * @param allocation Array representing the lines and their current position
 * @param length Length of the allocation array
 *
 * @return Returns true if the inputs have the right length and the action can be performed
 */
  bool permute_inputs_and_constant_inputs(circuit& circ, unsigned* allocation, unsigned length){
    if(circ.lines() != length){
      return false;
    }

    std::vector<std::string> in = circ.inputs();
    std::vector<std::string> new_in( length );
    std::vector<constant> constants = circ.constants();
    std::vector<constant> new_constants ( length );

    for ( unsigned u = 0; u<length; u++ ){
      new_in[u] = in[allocation[u]];
      if ( constants[u] )
        new_constants[u] = constants[allocation[u]];
    }
    circ.set_inputs(new_in);
    circ.set_constants(new_constants);

    return true;
  }

  /**
   * @brief Modifies the allocation by swaping two entries in the allocation table
   *
   * @param allocation Array representing the lines and their current position
   * @param l1 first line to swap
   * @param l2 second line to swap
   * @param length Length of the allocation array
   */
  inline void modify_allocation(unsigned* allocation, unsigned l1, unsigned l2, unsigned length ){
    if(l1 < length && l2 < length){
      //switch allocation table entries
      allocation[l2] += allocation[l1];
      allocation[l1] =  allocation[l2]-allocation[l1];
      allocation[l2] -= allocation[l1];
    }
  }

/**
 * @brief applies the optimization of the heuristic one-side-swapping lnn_optimization_mode on a circuit wrt. moving the target line towards the control line or the control line towards the target line
 *
 * @param circ Target circuit of the performing action
 * @param base Source circuit providing metadata and gates
 * @param towardstarget this param is set if the control lines should be moved by swap gates towards the target line
 *
 * @return the number of swaps in the circuit
 */
  unsigned apply_directed_local_reordering_scheme(circuit& circ, const circuit& base, bool towardstarget){
    copy_metadata(base,circ);
    unsigned num_swap_gates = 0;

    unsigned* allocation = init_allocation(base.lines());

    for ( const gate& g : base ){
      unsigned nnc_val = nnc(g, allocation, base.lines());
      if(nnc_val>0){

        auto ctrltar = lookup2(g.controls().front().line(),g.targets().front(), allocation, base.lines());

        bool direction = towardstarget ?
          (ctrltar.first > ctrltar.second):
          (ctrltar.first < ctrltar.second);
        unsigned s = towardstarget ? ctrltar.first : ctrltar.second;

        for(unsigned i = 0; i < nnc_val; i++){
          auto swap = direction ?
            std::make_pair(s - i , s - 1 -i) :
            std::make_pair(s + i , s + 1 +i) ;

          append_swap_gate(circ , swap.first, swap.second);
          num_swap_gates++;
          modify_allocation(allocation, swap.first, swap.second, base.lines());
        }

        circ.append_gate() = transform_gate2(g, allocation, base.lines());

      }
      else
        circ.append_gate() = transform_gate2(g, allocation, base.lines());
    }

    //clean up
    permute_outputs_and_garbage_lines(circ, allocation, base.lines());
    delete allocation;

    return num_swap_gates;
  }

/**
 * @brief applies the optimization of the heuristic one-side-swapping lnn_optimization_mode on a circuit.
 *
 * @param circ Target circuit of the performing action
 * @param base Source circuit providing metadata and gates
 *
 * @return the nnc of the circuit
 */
  unsigned apply_local_reordering_scheme(circuit& circ, const circuit& base){
    circuit c1;
    circuit c2;
    copy_metadata(base,c1);
    copy_metadata(base,c2);
    unsigned cost1 = apply_directed_local_reordering_scheme(c1, base, true);
    unsigned cost2 = apply_directed_local_reordering_scheme(c2, base, false);
    copy_circuit( cost1 < cost2 ? c1 : c2 ,circ);
    return (cost1 < cost2 ? cost1 : cost2);
  }

  void switch_lines(circuit& circ, const circuit& base, unsigned l1, unsigned l2 ){
    for( gate cg : base) {
      gate g(cg);
      switch( cg.size() ){
      case 2:
        if(cg.controls().front().line() == l1)
          g = set_gate_control_line(cg,l2);
        else if (cg.controls().front().line() == l2 )
          g = set_gate_control_line(cg,l1);
        //no break - check target line in case 1
      case 1:
        if( cg.targets().front() == l1)
          g = set_gate_control_line(g,l2);
        else if( cg.targets().front() == l2 )
          g = set_gate_control_line(g,l1);
        circ.append_gate() = g;
        break;
      }
    }

    //change circuit metadata: inputs, outputs, constant and garbage lines
    std::vector<std::string> out = base.outputs();
    std::string tmp_out;
    std::vector<std::string> in = base.inputs();
    std::string tmp_in;
    std::vector<bool> garbage = base.garbage();
    bool tmp_garbage;
    std::vector<constant> constants = base.constants();
    constant tmp_constants;

    tmp_out = out[l1];
    out[l1] = out[l2];
    out[l2] = tmp_out;

    tmp_in = in[l1];
    in[l1] = in[l2];
    in[l2] = tmp_in;

    tmp_garbage = garbage[l1];
    garbage[l1] = garbage[l2];
    garbage[l2] = tmp_garbage;

    tmp_constants = constants[l1];
    constants[l1] = constants[l2];
    constants[l2] = tmp_constants;

    circ.set_inputs(in);
    circ.set_constants(constants);
    circ.set_outputs(out);
    circ.set_garbage(garbage);
  }

  /**
   * @brief calculates the impact of the lines. More precisely, for each gate g with
     control line i and target line j, the NNC value is calculated.
     This value is added to variables imp_i and imp_j which are used
     to save the impacts of the circuit lines i and j on the total NNC
     value, respectively.
   *
   * @param circ the circuit for which the impact should be calculated
   *
   * @return a vector mapping the impact to a specific line
   */
  std::vector< std::pair<unsigned,unsigned> > calculate_impact(const circuit& circ){
    std::vector< std::pair<unsigned,unsigned> > impact;
    for(unsigned line_index=0; line_index < circ.lines(); line_index++)
      impact.push_back(std::make_pair(0,line_index));

    //calculate impact
    for ( const gate& g : circ) {
      unsigned nnc_val = nnc(g);
      if(nnc_val>0){
        impact[g.controls().front().line()].first +=nnc_val;
        impact[g.targets().front()].first +=nnc_val;
      }
    }
    impact.shrink_to_fit();
    return impact;
  }

  /**
   * @brief Function switching a line with a middle line
   *
   * @param circ Target circuit of the performing action
   * @param base Source circuit providing metadata and gates
   * @param
   *
   * return the nnc of the circuit
   */
  unsigned switch_lines(circuit& circ, const circuit& base, unsigned proposedline){
    unsigned middle = base.lines()/2;
    if(base.lines()%2){
      switch_lines(circ, base, proposedline, middle);
      return NNC(circ);
    }
    else{
      //two middle lines
      circuit circ2;
      copy_metadata(base,circ2);
      switch_lines(circ, base, proposedline, middle);
      switch_lines(circ2, base, proposedline, middle-1);
      unsigned NNC1 = NNC(circ);
      unsigned NNC2 = NNC(circ2);

      if(NNC2 < NNC1){
        circ = circ2;
        return NNC2;
      }
      return NNC1;
    }
  }

/**
 * @brief heuristic method for reordering the lines of a circuit, with the intention moving the line with the highest NNC impact in the middle
 *
 * @param circ Target circuit of the performing action
 * @param base Source circuit providing metadata and gates
 *
 * @return the nnc of the circuit
 */
  unsigned global_reorder_circuit(circuit& circ, const circuit& base){
    auto impact = calculate_impact(base);
    std::sort(impact.begin(), impact.end(), [](std::pair<unsigned,unsigned> a, std::pair<unsigned,unsigned> b){ return a.first > b.first; });
    //multiple lines may have the same highest impact (unspecified in the algorithm)
    std::vector<unsigned> max_lines = [](std::vector<std::pair<unsigned,unsigned>> input){
      unsigned max = input[0].first;
      std::vector<unsigned> out;
      for(std::pair<unsigned,unsigned> p : input)
        if(p.first == max)
          out.push_back(p.second);
      return out;
    }(impact);

    unsigned best_nnc = UINT_MAX;
    for(unsigned line : max_lines){
      if (max_lines.size() < impact.size() && (line == base.lines()/2 || line+1 == base.lines()/2))
        line = impact[max_lines.size()].second; //select line with the next highest impact
      circuit circ2;
      copy_metadata(base,circ2);
      unsigned circ_nnc = switch_lines(circ2, base, line);
      if(circ_nnc < best_nnc){
        circ = circ2;
        best_nnc = circ_nnc;
      }
    }
    return best_nnc;
  }

  /**
   * @brief An ordering of the circuit lines which reduces the total NNC value
     is desired. To do that, the contribution of each line to the total
     NNC value is calculated. More precisely, for each gate g with
     control line i and target line j, the NNC value is calculated.
     This value is added to variables imp_i and imp_j which are used
     to save the impacts of the circuit lines i and j on the total NNC
     value, respectively.
     Next, the line with the highest NNC impact is chosen for
     reordering and placed at the middle line (i.e., swapped with the
     middle line). If the selected line is the middle line itself, a
     line with the next highest impact is selected. This procedure is
     repeated until no better NNC value is achieved.
  *
  * @param circ Target circuit of the performing action
  * @param base Source circuit providing metadata and gates
  *
  * @return the nnc of the circuit
  */
unsigned apply_global_reordering_scheme( circuit& circ, const circuit& base) {
  copy_metadata(base,circ);
  unsigned circ_nnc = global_reorder_circuit(circ, base);
  while (true){
    circuit circ2;
    copy_metadata(base,circ2);
    unsigned circ2_nnc = global_reorder_circuit(circ2, circ);
    if(circ2_nnc < circ_nnc){
      circ = circ2;
      circ_nnc = circ2_nnc;
    }
    else
      return 2*circ_nnc;
  }
}

  /**
   * @brief Applies the linear nearest neighbor optimization to a quantum circuit. Different modes are possible.
      The global and local reordering scheme have been introduced in [\ref SWD10].
   *  reordering mode:
   *  1: naive, 2: local reordering, 3: global reordering
   *
   */
  bool lnn_optimization( circuit& circ, const circuit& base, properties::ptr settings, properties::ptr statistics )
  {
    /* settings */
    bool verbose = get(settings, "verbose", false);
    /*  1: naive, 2: local reordering, 3: global reordering */
    unsigned reordering_mode = get<unsigned>(settings, "reordering_mode", 0u);

    // Run-time measuring
    properties_timer t( statistics );

    unsigned number_of_swaps;
    switch(reordering_mode) {
    case LNN_OPTIMIZATION_NONE:
      copy_circuit(base,circ);
      number_of_swaps = NNC(base)*2;
      break;
    case LNN_OPTIMIZATION_NAIVE:
      if(NNC(base) == 0)
	copy_circuit(base,circ);
      else
	number_of_swaps = apply_naive_scheme(circ,base);
      break;
    case LNN_OPTIMIZATION_LOCAL_REORDER:
      if(NNC(base) == 0)
	copy_circuit(base,circ);
      else
	number_of_swaps = apply_local_reordering_scheme(circ, base);
      break;
    case LNN_OPTIMIZATION_GLOBAL_REORDER:
      number_of_swaps = apply_global_reordering_scheme(circ, base);
      break;

    default:
      if(verbose)
	std::cout << "invalid mode." << std::endl;
      return false;
    }

    if(verbose)
      std::cout << "SWAP gates: " << number_of_swaps << std::endl;

    return true;
}

optimization_func lnn_optimization_func( properties::ptr settings, properties::ptr statistics )
{
    optimization_func f = [&settings, &statistics]( circuit& circ, const circuit& base ) {
      return lnn_optimization( circ, base, settings, statistics );
    };
    f.init( settings, statistics );
    return f;
}
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include <Core/Geometry/Approximation/VariationalShapeApproximation.hpp>

#include <set>
#include <random>
#include <Eigen/Eigenvalues>
#include <Core/Log/Log.hpp>
#include <Core/Geometry/Triangle/TriangleOperation.hpp>

namespace Ra {
namespace Core {
namespace Geometry {

//////////////////////////////////////////////////////////////////////////////
// CONSTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline VariationalShapeApproximationBase< K_Region >::VariationalShapeApproximationBase( const Mesh& mesh ) :
    m_mesh( mesh ),
    m_queue(),
    m_region(),
    m_proxy(),
    m_init( false ) {}

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline VariationalShapeApproximationBase< K_Region >::~VariationalShapeApproximationBase() {}

//////////////////////////////////////////////////////////////////////////////
// INIT
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::init() {
    this->compute_data();
    this->compute_seed();
    this->m_init = true;
}

template < uint K_Region >
inline bool VariationalShapeApproximationBase< K_Region >::initialized() const {
    return m_init;
}

//////////////////////////////////////////////////////////////////////////////
// EXECUTION
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::exec( const uint iteration ) {
    if( !this->initialized() ) {
        CORE_WARN_IF( false, "V.S.A. NOT INITIALIZED" );
        return;
    }
    LOG( logDEBUG ) << "Computing V.S.A. ...";
    for( uint i = 0; i < iteration; ++i )  {
        this->proxy_fitting();
        this->geometry_partitioning();
    }
    LOG( logDEBUG ) << "V.S.A. completed.";
}

template < uint K_Region >
template < uint Iteration >
inline void VariationalShapeApproximationBase< K_Region >::exec() {
    if( !this->initialized() ) {
        CORE_WARN_IF( false, "V.S.A. NOT INITIALIZED" );
        return;
    }
    LOG( logDEBUG ) << "Computing V.S.A. ...";
    for( uint i = 0; i < Iteration; ++i )  {
        this->proxy_fitting();
        this->geometry_partitioning();
    }
    LOG( logDEBUG ) << "V.S.A. completed.";
}

//////////////////////////////////////////////////////////////////////////////
// REGION
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline const typename VariationalShapeApproximationBase< K_Region >::Region& VariationalShapeApproximationBase< K_Region >::region( const uint i ) const {
    return m_region.at( i );
}

//////////////////////////////////////////////////////////////////////////////
// PROXY
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline const typename VariationalShapeApproximationBase< K_Region >::Proxy& VariationalShapeApproximationBase< K_Region >::proxy( const uint i ) const {
    return m_proxy.at( i );
}

//////////////////////////////////////////////////////////////////////////////
// COLOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::create_region_color() {
    for( uint i = 0; i < K; ++i ) {
        const Scalar q = Scalar( i ) / static_cast< Scalar >( K - 1 );
        for( const auto& T : this->m_region[i] ) {
            this->m_fvalue[T]  = q;
        }
    }
}

template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::create_energy_color() {
    for( uint i = 0; i < K; ++i ) {
        for( const auto& T : this->m_region[i] ) {
            this->m_fvalue[T]  = this->E( T, this->m_proxy[i] );
        }
    }
}

template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::shuffle_regions() {
    //This function was made in order to have a chance for neighboring regions
    //to have distant IDs, hence having a different color.
    std::array< uint, K > id;
    RegionList tmp_region;
    ProxyList  tmp_proxy;
    for( uint i = 0; i < K; ++i ) {
        id[i] = i;
    }
    std::random_shuffle( id.begin(), id.end() );
    for( uint i = 0; i < K; ++i ) {
        tmp_region[i] = this->m_region[id[i]];
        tmp_proxy[i]  = this->m_proxy[id[i]];
    }
    std::swap( tmp_region, this->m_region );
    std::swap( tmp_proxy,  this->m_proxy  );
    for( uint i = 0; i < K; ++i ) {
        for( const auto& T : this->m_region[i] ) {
            this->m_fregion[T] = i;
        }
    }
}

//////////////////////////////////////////////////////////////////////////////
// DATA
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::compute_data() {
    const uint size = this->m_mesh.m_triangles.size();
    this->m_fbary.resize( size );
    this->m_fnormal.resize( size );
    this->m_farea.resize( size );
    this->m_fregion.resize( size, -1 );
    this->m_fvisited.resize( size, false );
    this->m_fvalue.resize( 0 );
    this->m_fadj.resize( size );

    std::vector< std::vector< TriangleIdx > > vadj( this->m_mesh.m_vertices.size() );
    for( uint t = 0; t < size; ++t ) {
        const Triangle& T    = this->m_mesh.m_triangles[t];
        const uint      i    = T[0];
        const uint      j    = T[1];
        const uint      k    = T[2];
        const Vector3   v[3] = { this->m_mesh.m_vertices[i], this->m_mesh.m_vertices[j], this->m_mesh.m_vertices[k] };
        this->m_fbary[t] = triangleBarycenter( v[0], v[1], v[2] );
        this->m_fnormal[t] = triangleNormal( v[0], v[1], v[2] );
        this->m_farea[t] = triangleArea( v[0], v[1], v[2] );
        vadj[i].push_back( t );
        vadj[j].push_back( t );
        vadj[k].push_back( t );
    }

    for( const auto& list : vadj ) {
        for( uint i = 0; i < list.size(); ++i ) {
            for( uint j = i+1; j < list.size(); ++j ) {
                this->m_fadj[list[i]].insert( list[j] );
                this->m_fadj[list[j]].insert( list[i] );
            }
        }
    }

}

//////////////////////////////////////////////////////////////////////////////
// SEED
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::compute_seed() {
    std::set< TriangleIdx > t;
    std::default_random_engine g(time(0));
    std::uniform_int_distribution< TriangleIdx > rnd( 0, this->m_mesh.m_triangles.size()-1 );
    while( t.size() != K ) {
        t.insert( rnd(g) );
    }
    for( uint i = 0; i < K; ++i ) {
        m_region[i].push_back( *t.begin() );
        t.erase( t.begin() );
    }
}

//////////////////////////////////////////////////////////////////////////////
// GEOMETRY PARTITIONING
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::geometry_partitioning() {
    //Reset the visited flag
    for( TriangleIdx T = 0; T < this->m_mesh.m_triangles.size(); ++T ) {
        this->m_fvisited[T] = false;
    }

    //Force the queue to be empty
    this->m_queue = PriorityQueue();

    //Choose best triangle in each region
    for( uint i = 0; i < K; ++i ) {
        assert( !this->m_region[i].empty() );
        Energy min_e = std::numeric_limits< Energy >::max();
        TriangleIdx T;
        for( const auto& t : this->m_region[i] ) {
            const Energy e = this->E( t, this->m_proxy[i] );
            if( e < min_e ) {
                min_e = e;
                T = t;
            }
        }
        this->add_neighbors_to_queue( T, i );

        // Clear the region
        this->m_region[i].clear();
        this->m_region[i].push_back( T );
        this->m_fregion[T]  = i;
        this->m_fvisited[T] = true;
    }

    //While the queue is not empty
     while( !this->m_queue.empty() ) {
         const auto q = this->m_queue.top();
         this->m_queue.pop();
         const TriangleIdx t = q.second.first;
         if( this->m_fvisited[t] ) {
             continue;
         }
         const uint R = q.second.second;
         this->m_region[R].push_back( t );
         this->m_fregion[t]  = R;
         this->m_fvisited[t] = true;

         this->add_neighbors_to_queue( t, R );
     }
}

template < uint K_Region >
inline void VariationalShapeApproximationBase< K_Region >::add_neighbors_to_queue( const TriangleIdx& T, const uint proxy_id ) {
    for( const auto& r : this->m_fadj[T] ) {
        this->m_queue.push( QueueEntry( this->E( r, this->m_proxy[proxy_id] ), Pair( r, proxy_id ) ) );
    }
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region, MetricType Type >
VariationalShapeApproximation< K_Region, Type >::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region, MetricType Type >
inline void VariationalShapeApproximation< K_Region, Type >::proxy_fitting() {
    static const Scalar c = 2.0 / 72.0;
    Matrix3 m;
    m << 10.0, 7.0, 0.0, 7.0, 10.0, 0.0, 0.0, 0.0, 0.0;
    for( uint i = 0; i < this->K; ++i ) {

        Scalar area_sum         = 0;
        this->m_proxy[i].first  = Vector3::Zero();
        Matrix3 Q               = Matrix3::Zero();

        for( const auto& T : this->m_region[i] ) {
            const Vector3  v[3] = { this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][0]],
                                    this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][1]],
                                    this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][2]] };
            const Vector3& g    = this->m_fbary[T];

            Matrix3 M;
            M.row(0) = v[1] - v[0];
            M.row(1) = v[2] - v[0];
            M.row(2) = Vector3::Zero();

            Q += ( c * this->m_farea[T] * M * m * M.transpose() ) + ( this->m_farea[T] * g * g.transpose() );

            this->m_proxy[i].first += this->m_farea[T] * g;
            area_sum               += this->m_farea[T];
        }
        this->m_proxy[i].first /= area_sum;
        Q -= area_sum * this->m_proxy[i].first * this->m_proxy[i].first.transpose();
        this->m_proxy[i].second = Eigen::SelfAdjointEigenSolver<Matrix3>( Q ).eigenvectors().col( 0 ).normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region, MetricType Type >
inline Scalar VariationalShapeApproximation< K_Region, Type >::E( const TriangleIdx& T, const Proxy& P ) const {
    static const Scalar c = 1.0 / 6.0;
    const Vector3 v[3] = { this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][0]],
                           this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][1]],
                           this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][2]] };
    const Vector3 p    = P.first;
    const Vector3 n    = P.second;
    Scalar d[3];
    d[0] = std::abs( n.dot( p - v[0] ) );
    d[1] = std::abs( n.dot( p - v[1] ) );
    d[2] = std::abs( n.dot( p - v[2] ) );
    return ( c * ( ( d[0] * d[0] ) +
                   ( d[1] * d[1] ) +
                   ( d[2] * d[2] ) +
                   ( d[0] * d[1] ) +
                   ( d[0] * d[2] ) +
                   ( d[1] * d[2] ) ) * this->m_farea[T] );
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
VariationalShapeApproximation< K_Region, MetricType::L21 >::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximation< K_Region, MetricType::L21 >::proxy_fitting() {
    for( uint i = 0; i < this->K; ++i ) {
        Scalar area_sum = 0;
        this->m_proxy[i].first  = Vector3::Zero();
        this->m_proxy[i].second = Vector3::Zero();
        for( const auto& T : this->m_region[i] ) {
            this->m_proxy[i].first  += this->m_farea[T] * this->m_fbary[T];
            this->m_proxy[i].second += this->m_farea[T] * this->m_fnormal[T];
            area_sum                += this->m_farea[T];
        }
        this->m_proxy[i].first  /= area_sum;
        this->m_proxy[i].second = this->m_proxy[i].second.normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline Scalar VariationalShapeApproximation< K_Region, MetricType::L21 >::E( const TriangleIdx& T, const Proxy& P ) const {
    const Scalar  a      = this->m_farea[T];
    const Vector3 n      = this->m_fnormal[T];
    const Vector3 d      = n - P.second;
    const Scalar  result = a * d.squaredNorm();
    return result;
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
VariationalShapeApproximation< K_Region, MetricType::LLOYD >::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline void VariationalShapeApproximation< K_Region, MetricType::LLOYD >::proxy_fitting() {
    for( uint i = 0; i < this->K; ++i ) {
        this->m_proxy[i].first  = Vector3::Zero();
        this->m_proxy[i].second = Vector3::Zero();
        for( const auto& T : this->m_region[i] ) {
            this->m_proxy[i].first  += this->m_fbary[T];
            this->m_proxy[i].second += this->m_fnormal[T];
        }
        this->m_proxy[i].first  /= this->m_region[i].size();
        this->m_proxy[i].second  = this->m_proxy[i].second.normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template < uint K_Region >
inline Scalar VariationalShapeApproximation< K_Region, MetricType::LLOYD >::E( const TriangleIdx& T, const Proxy& P ) const {
    const Vector3& b     = this->m_fbary[T];
    const Scalar  result = (b - P.first).norm();
    return result;
}

} // namespace Geometry
} // namespace Core
} // namespace Ra


#pragma once

#include <algorithm>
#include <cstddef>
#include <functional>
#include <initializer_list>
#include <iterator>
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include <binary_quadratic_model.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/functional/hash.hpp>
#include <robin_hood.h>

#include "abstract_syntax_tree.hpp"

namespace pyqubo {
  class variables final {
    robin_hood::unordered_map<std::string, int> _indexes;
    robin_hood::unordered_map<int, std::string> _names;

  public:
    variables() noexcept : _indexes{}, _names{} {
      ;
    }

    auto index(const std::string& variable_name) noexcept {
      const auto [it, emplaced] = _indexes.emplace(variable_name, std::size(_indexes));

      if (emplaced) {
        _names.emplace(it->second, variable_name);
      }

      return it->second;
    }

    const auto& name(int index) const noexcept {
      return _names.find(index)->second;
    }

    auto names() const noexcept {
      auto result = std::vector<std::string>(std::size(_names));

      for (const auto& [index, name] : _names) {
        result[index] = name;
      }

      return result;
    }
  };

  using indexes = boost::container::small_vector<int, 2>;

  class product final {
    pyqubo::indexes _indexes;
    std::size_t _hash;

  public:
    product(const pyqubo::indexes& indexes) noexcept : _indexes(indexes), _hash(boost::hash_range(std::begin(indexes), std::end(indexes))) {
      ;
    }

    product(std::initializer_list<int> init) noexcept : product(pyqubo::indexes(init)) {
      ;
    }

    const auto& indexes() const noexcept {
      return _indexes;
    }

    friend std::hash<product>;
  };

  inline auto operator*(const product& product_1, const product& product_2) noexcept {
    return product([&] {
      auto result = indexes{};

      std::set_union(std::begin(product_1.indexes()), std::end(product_1.indexes()),
                     std::begin(product_2.indexes()), std::end(product_2.indexes()),
                     std::back_inserter(result));

      return result;
    }());
  }

  inline bool operator==(const product& product_1, const product& product_2) noexcept {
    return product_1.indexes() == product_2.indexes();
  }
}

namespace std {
  template <>
  struct hash<pyqubo::product> {
    auto operator()(const pyqubo::product& product) const noexcept {
      return product._hash;
    }
  };
}

namespace pyqubo {
  // std::variantを使用してzeroやmonomialな場合の処理削減をやってみたのですが、パフォーマンスは向上しませんでした。なので、unordered_map一本でやります。
  // よく考えれば、要素数が0の場合の処理とかはunordered_mapの中でやっていそうですし。。。

  using polynomial = robin_hood::unordered_map<product, std::shared_ptr<const expression>>;

  inline auto operator+(const polynomial& polynomial_1, const polynomial& polynomial_2) noexcept {
    auto result = polynomial_1;

    for (const auto& [product, coefficient] : polynomial_2) {
      const auto [it, emplaced] = result.emplace(product, coefficient);

      if (!emplaced) {
        it->second = it->second + coefficient;
      }
    }

    return result;
  }

  inline auto operator*(const polynomial& polynomial_1, const polynomial& polynomial_2) noexcept {
    auto result = polynomial{};

    for (const auto& [product_1, coefficient_1] : polynomial_1) {
      for (const auto& [product_2, coefficient_2] : polynomial_2) {
        const auto [it, emplaced] = result.emplace(product_1 * product_2, coefficient_1 * coefficient_2);

        if (!emplaced) {
          it->second = it->second + coefficient_1 * coefficient_2;
        }
      }
    }

    return result;
  }

  class evaluate final {
    std::unordered_map<std::string, double> _feed_dict;

  public:
    evaluate(const std::unordered_map<std::string, double>& feed_dict) noexcept : _feed_dict(feed_dict) {
      ;
    }

    auto operator()(const std::shared_ptr<const expression>& expression) const noexcept {
      return visit<double>(*this, expression);
    }

    auto operator()(const std::shared_ptr<const add_operator>& add_operator) const noexcept {
      return std::accumulate(std::begin(add_operator->children()), std::end(add_operator->children()), 0.0, [&](const auto& acc, const auto& child) {
        return acc + visit<double>(*this, child);
      });
    }

    auto operator()(const std::shared_ptr<const mul_operator>& mul_operator) const noexcept {
      return visit<double>(*this, mul_operator->lhs()) * visit<double>(*this, mul_operator->rhs());
    }

    auto operator()(const std::shared_ptr<const placeholder_variable>& place_holder_variable) const noexcept {
      return _feed_dict.at(place_holder_variable->name());
    }

    auto operator()(const std::shared_ptr<const user_defined_expression>& user_defined_expression) const noexcept {
      return visit<double>(*this, user_defined_expression->expression());
    }

    auto operator()(const std::shared_ptr<const numeric_literal>& numeric_literal) const noexcept {
      return numeric_literal->value();
    }
  };

  class solution final {
    std::unordered_map<std::string, int> _sample;
    double _energy;
    std::unordered_map<std::string, double> _sub_hamiltonians;
    std::unordered_map<std::string, std::pair<bool, double>> _constraints;

  public:
    solution(const std::unordered_map<std::string, int> sample, double energy, const std::unordered_map<std::string, double>& sub_hamiltonians, const std::unordered_map<std::string, std::pair<bool, double>>& constraints) noexcept : _sample(sample), _energy(energy), _sub_hamiltonians(sub_hamiltonians), _constraints(constraints) {
      ;
    }

    const auto& sample() const noexcept {
      return _sample;
    }

    auto energy() const noexcept {
      return _energy;
    }

    const auto& sub_hamiltonians() const noexcept {
      return _sub_hamiltonians;
    }

    const auto& constraints() const noexcept {
      return _constraints;
    }
  };

  class model final {
    polynomial _quadratic_polynomial;
    robin_hood::unordered_map<std::string, polynomial> _sub_hamiltonians;
    robin_hood::unordered_map<std::string, std::pair<polynomial, std::function<bool(double)>>> _constraints;
    variables _variables;

    static auto to_cimod_vartype(const std::string vartype) noexcept {
      return vartype == "BINARY" ? cimod::Vartype::BINARY : cimod::Vartype::SPIN;
    }

  public:
    model(const polynomial& quadratic_polynomial, const robin_hood::unordered_map<std::string, polynomial>& sub_hamiltonians, const robin_hood::unordered_map<std::string, std::pair<polynomial, std::function<bool(double)>>>& constraints, const variables& variables) noexcept : _quadratic_polynomial(quadratic_polynomial), _sub_hamiltonians(sub_hamiltonians), _constraints(constraints), _variables(variables) {
      ;
    }

    std::vector<std::string> variable_names() const noexcept {
      return _variables.names();
    }

    template <typename T = std::string>
    auto to_bqm_parameters(const std::unordered_map<std::string, double>& feed_dict) const noexcept { // 不格好でごめんなさい。PythonのBinaryQuadraticModelを作成可能にするために、このメンバ関数でBinaryQuadraticModelの引数を生成します。
      const auto evaluate = pyqubo::evaluate(feed_dict);

      auto linear = cimod::Linear<T, double>{};
      auto quadratic = cimod::Quadratic<T, double>{};
      auto offset = 0.0;

      for (const auto& [product, coefficient] : _quadratic_polynomial) {
        const auto coefficient_value = evaluate(coefficient);

        switch (std::size(product.indexes())) {
        case 0: {
          offset = coefficient_value; // 次数が0の項は1つにまとめられるので、この処理は最大で1回しか実行されません。なので、+=ではなくて=を使用しています。
          break;
        }
        case 1: {
          linear.emplace(_variables.name(product.indexes()[0]), coefficient_value);
          break;
        }
        case 2: {
          quadratic.emplace(std::pair{_variables.name(product.indexes()[0]), _variables.name(product.indexes()[1])}, coefficient_value);
          break;
        }
        default:
          throw std::runtime_error("invalid term."); // ここには絶対にこないはず。
        }
      }

      return std::tuple{linear, quadratic, offset};
    }

    template <typename T = std::string>
    auto to_bqm(const std::unordered_map<std::string, double>& feed_dict, cimod::Vartype vartype) const noexcept {
      const auto [linear, quadratic, offset] = to_bqm_parameters<T>(feed_dict);

      return cimod::BinaryQuadraticModel<T, double, cimod::Dense>(linear, quadratic, offset, vartype);
    }

    template <typename T = std::string>
    auto energy(const std::unordered_map<T, int>& sample, const std::string& vartype, const std::unordered_map<std::string, double>& feed_dict) const noexcept {
      return to_bqm<T>(feed_dict, to_cimod_vartype(vartype)).energy([&] {
        // BinaryQuadraticModelの引数でvartypeを設定しても、energyでは使われないみたい。。。Determine the energy of the specified sample of a binary quadratic modelって書いてある。
        // しょうがないので、spinからbinaryに変換します。

        auto result = sample;

        if (vartype == "SPIN") {
          for (const auto& [name, value] : sample) {
            result[name] = (value + 1) / 2;
          }
        }

        return result;
      }());
    }

    template <typename T = std::string>
    auto decode_sample(const std::unordered_map<T, int>& sample, const std::string& vartype, const std::unordered_map<std::string, double>& feed_dict) const noexcept {
      const auto evaluate = pyqubo::evaluate(feed_dict);
      const auto evaluate_polynomial = [&](const auto& polynomial, const auto& sample) {
        return std::accumulate(std::begin(polynomial), std::end(polynomial), 0.0, [&](const auto acc, const auto& term) {
          return acc +
                 std::accumulate(std::begin(term.first.indexes()), std::end(term.first.indexes()), 1, [&](const auto acc, const auto& index) {
                   const auto value = sample.at(_variables.name(index));

                   return acc * (vartype == "BINARY" ? value : (value + 1) / 2);
                 }) * evaluate(term.second);
        });
      };

      return solution(
          sample,
          energy<T>(sample, vartype, feed_dict),
          [&] {
            auto result = std::unordered_map<std::string, double>{};

            for (const auto& [name, polynomial] : _sub_hamiltonians) {
              result.emplace(name, evaluate_polynomial(polynomial, sample));
            }

            return result;
          }(),
          [&] {
            auto result = std::unordered_map<std::string, std::pair<bool, double>>{};

            for (const auto& [name, pair] : _constraints) {
              const auto& [polynomial, condition] = pair;
              const auto energy = evaluate_polynomial(polynomial, sample);

              result.emplace(name, std::pair{condition(energy), energy});
            }

            return result;
          }());
    }

    template <typename T = std::string>
    auto decode_samples(const std::vector<std::unordered_map<T, int>>& samples, const std::string& vartype, const std::unordered_map<std::string, double>& feed_dict) const noexcept {
      auto result = std::vector<solution>{};

      std::transform(std::begin(samples), std::end(samples), std::back_inserter(result), [&](const auto& sample) {
        return decode_sample(sample, vartype, feed_dict);
      });

      return result;
    }
  };

  template <>
  inline auto model::to_bqm_parameters<int>(const std::unordered_map<std::string, double>& feed_dict) const noexcept { // メンバ関数を特殊化するときは、クラスの外に書かなければなりません。。。
    const auto evaluate = pyqubo::evaluate(feed_dict);

    auto linear = cimod::Linear<int, double>{};
    auto quadratic = cimod::Quadratic<int, double>{};
    auto offset = 0.0;

    for (const auto& [product, coefficient] : _quadratic_polynomial) {
      const auto coefficient_value = evaluate(coefficient);

      switch (std::size(product.indexes())) {
      case 0: {
        offset = coefficient_value; // 次数が0の項は1つにまとめられるので、この処理は最大で1回しか実行されません。なので、+=ではなくて=を使用しています。
        break;
      }
      case 1: {
        linear.emplace(product.indexes()[0], coefficient_value);
        break;
      }
      case 2: {
        quadratic.emplace(std::pair{product.indexes()[0], product.indexes()[1]}, coefficient_value);
        break;
      }
      default:
        throw std::runtime_error("invalid term."); // ここには絶対にこないはず。
      }
    }

    return std::tuple{linear, quadratic, offset};
  }

  template <>
  inline auto model::decode_sample<int>(const std::unordered_map<int, int>& sample, const std::string& vartype, const std::unordered_map<std::string, double>& feed_dict) const noexcept {
    const auto evaluate = pyqubo::evaluate(feed_dict);
    const auto evaluate_polynomial = [&](const auto& polynomial, const auto& sample) {
      return std::accumulate(std::begin(polynomial), std::end(polynomial), 0.0, [&](const auto acc, const auto& term) {
        return acc +
               std::accumulate(std::begin(term.first.indexes()), std::end(term.first.indexes()), 1, [&](const auto acc, const auto& index) {
                 const auto value = sample.at(index);

                 return acc * (vartype == "BINARY" ? value : (value + 1) / 2);
               }) * evaluate(term.second);
      });
    };

    return solution(
        [&] {
          auto result = std::unordered_map<std::string, int>{};

          std::transform(std::begin(sample), std::end(sample), std::inserter(result, std::begin(result)), [&](const auto& index_and_value) {
            return std::pair{_variables.name(index_and_value.first), index_and_value.second};
          });

          return result;
        }(),
        energy<int>(sample, vartype, feed_dict),
        [&] {
          auto result = std::unordered_map<std::string, double>{};

          for (const auto& [name, polynomial] : _sub_hamiltonians) {
            result.emplace(name, evaluate_polynomial(polynomial, sample));
          }

          return result;
        }(),
        [&] {
          auto result = std::unordered_map<std::string, std::pair<bool, double>>{};

          for (const auto& [name, pair] : _constraints) {
            const auto& [polynomial, condition] = pair;
            const auto energy = evaluate_polynomial(polynomial, sample);

            result.emplace(name, std::pair{condition(energy), energy});
          }

          return result;
        }());
  }
}


/*****************************************************************************
*
* Copyright (c) 2016 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

#include <trilinoswrap/PreconditionerFactory.h>
#include <trilinoswrap/TrilinosAdapterException.h>
#include <trilinoswrap/util.h>

#include <escript/SolverOptions.h>

#include <Ifpack2_Factory.hpp>
#if 1 //ndef ESYS_INDEXTYPE_LONG
#include <MueLu_CreateTpetraPreconditioner.hpp>
#endif

#include <boost/python/dict.hpp>

using Teuchos::RCP;

namespace bp = boost::python;

namespace esys_trilinos {

template<typename ST>
RCP<OpType<ST> > createPreconditioner(RCP<const MatrixType<ST> > mat,
                                      const escript::SolverBuddy& sb)
{
    using util::extractParamIfSet;

    typedef MatrixType<ST> Matrix;

    RCP<Teuchos::ParameterList> params = Teuchos::parameterList();
    Ifpack2::Factory factory;
    RCP<OpType<ST> > prec;
    RCP<Ifpack2::Preconditioner<ST,LO,GO,NT> > ifprec;
    const bp::dict& pyParams = sb.getTrilinosParameters();

    switch (sb.getPreconditioner()) {
        case escript::SO_PRECONDITIONER_NONE:
            break;
        case escript::SO_PRECONDITIONER_AMG:
            {
#if 1 //ndef ESYS_INDEXTYPE_LONG
                params->set("max levels", sb.getLevelMax());
                params->set("number of equations", 1);
                params->set("cycle type", sb.getCycleType()==1 ? "V" : "W");
                params->set("problem: symmetric", sb.isSymmetric());
                params->set("verbosity", sb.isVerbose()? "high":"none");
                // override parameters if set explicitly for trilinos
                // The set of available parameters is documented in the MueLu
                // user's guide (PDF download)
                // NOTE: passing sub parameter lists is not supported via
                // python due to escript's SolverBuddy constraints.
                // Use the 'xml parameter file' option instead.
                extractParamIfSet<std::string>("problem: type", pyParams, *params);
                extractParamIfSet<std::string>("verbosity", pyParams, *params);
                extractParamIfSet<int>("number of equations", pyParams, *params);
                extractParamIfSet<int>("max levels", pyParams, *params);
                extractParamIfSet<std::string>("cycle type", pyParams, *params);
                extractParamIfSet<bool>("problem: symmetric", pyParams, *params);
                extractParamIfSet<std::string>("xml parameter file", pyParams, *params);
                extractParamIfSet<std::string>("smoother: pre or post", pyParams, *params);
                extractParamIfSet<std::string>("smoother: type", pyParams, *params);
                extractParamIfSet<std::string>("smoother: pre type", pyParams, *params);
                extractParamIfSet<std::string>("smoother: post type", pyParams, *params);
                extractParamIfSet<int>("coarse: max size", pyParams, *params);
                extractParamIfSet<std::string>("coarse: type", pyParams, *params);
                extractParamIfSet<std::string>("aggregation: type", pyParams, *params);
                extractParamIfSet<std::string>("aggregation: ordering", pyParams, *params);
                extractParamIfSet<std::string>("aggregation: drop scheme", pyParams, *params);
                extractParamIfSet<ST>("aggregation: drop tol", pyParams, *params);
                extractParamIfSet<int>("aggregation: min agg size", pyParams, *params);
                extractParamIfSet<int>("aggregation: max agg size", pyParams, *params);
                extractParamIfSet<ST>("aggregation: Dirichlet threshold", pyParams, *params);
                extractParamIfSet<bool>("aggregation: export visualization data", pyParams, *params);
                extractParamIfSet<std::string>("aggregation: output filename", pyParams, *params);
                extractParamIfSet<int>("aggregation: output file: time step", pyParams, *params);
                extractParamIfSet<int>("aggregation: output file: iter", pyParams, *params);
                extractParamIfSet<std::string>("aggregation: output file: agg style", pyParams, *params);
                extractParamIfSet<bool>("aggregation: output file: fine graph edges", pyParams, *params);
                extractParamIfSet<bool>("aggregation: output file: coarse graph edges", pyParams, *params);
                extractParamIfSet<bool>("aggregation: output file: build colormap", pyParams, *params);
                extractParamIfSet<bool>("repartition: enable", pyParams, *params);
                extractParamIfSet<std::string>("repartition: partitioner", pyParams, *params);
                extractParamIfSet<int>("repartition: start level", pyParams, *params);
                extractParamIfSet<int>("repartition: min rows per proc", pyParams, *params);
                extractParamIfSet<ST>("repartition: max imbalance", pyParams, *params);
                extractParamIfSet<bool>("repartition: remap parts", pyParams, *params);
                extractParamIfSet<bool>("repartition: rebalance P and R", pyParams, *params);
                extractParamIfSet<std::string>("multigrid algorithm", pyParams, *params);
                extractParamIfSet<int>("semicoarsen: coarsen rate", pyParams, *params);
                extractParamIfSet<ST>("sa: damping factor", pyParams, *params);
                extractParamIfSet<bool>("sa: use filtered matrix", pyParams, *params);
                extractParamIfSet<bool>("filtered matrix: use lumping", pyParams, *params);
                extractParamIfSet<bool>("filtered matrix: reuse eigenvalue", pyParams, *params);
                extractParamIfSet<std::string>("emin: iterative method", pyParams, *params);
                extractParamIfSet<int>("emin: num iterations", pyParams, *params);
                extractParamIfSet<int>("emin: num reuse iterations", pyParams, *params);
                extractParamIfSet<std::string>("emin: pattern", pyParams, *params);
                extractParamIfSet<int>("emin: pattern order", pyParams, *params);
                extractParamIfSet<std::string>("reuse: type", pyParams, *params);
                extractParamIfSet<bool>("print initial parameters", pyParams, *params);
                extractParamIfSet<bool>("print unused parameters", pyParams, *params);
                extractParamIfSet<bool>("transpose: use implicit", pyParams, *params);
                RCP<OpType<ST> > A(Teuchos::rcp_const_cast<Matrix>(mat));
                prec = MueLu::CreateTpetraPreconditioner(A, *params);
#else
                throw escript::ValueError("MueLu (AMG) is incompatible with index type long!");
#endif
            }
            break;
        case escript::SO_PRECONDITIONER_ILUT:
            ifprec = factory.create<const Matrix>("ILUT", mat);
            params->set("fact: drop tolerance", sb.getDropTolerance());
            params->set("fact: relax value", sb.getRelaxationFactor());
            // override if set explicitly for trilinos
            extractParamIfSet<ST>("fact: relax value", pyParams, *params);
            extractParamIfSet<ST>("fact: drop tolerance", pyParams, *params);
            extractParamIfSet<int>("fact: ilut level-of-fill", pyParams, *params);
            extractParamIfSet<ST>("fact: absolute threshold", pyParams, *params);
            extractParamIfSet<ST>("fact: relative threshold", pyParams, *params);
            break;
        case escript::SO_PRECONDITIONER_GAUSS_SEIDEL:
        case escript::SO_PRECONDITIONER_JACOBI:
          {
            ifprec = factory.create<const Matrix>("RELAXATION", mat);
            if (sb.getPreconditioner() == escript::SO_PRECONDITIONER_JACOBI) {
                params->set("relaxation: type", "Jacobi");
            } else {
                params->set("relaxation: type", (sb.isSymmetric() ?
                            "Symmetric Gauss-Seidel" : "Gauss-Seidel"));
            }
            params->set("relaxation: sweeps", sb.getNumSweeps());
            const ST fac = static_cast<ST>(sb.getRelaxationFactor());
            params->set("relaxation: damping factor", fac);
            // override if set explicitly for trilinos
            extractParamIfSet<int>("relaxation: sweeps", pyParams, *params);
            extractParamIfSet<ST>("relaxation: damping factor", pyParams, *params);
            extractParamIfSet<ST>("relaxation: min diagonal value", pyParams, *params);
            extractParamIfSet<bool>("relaxation: zero starting solution", pyParams, *params);
            extractParamIfSet<bool>("relaxation: backward mode", pyParams, *params);
            break;
          }
        case escript::SO_PRECONDITIONER_ILU0: // to avoid test failures
        case escript::SO_PRECONDITIONER_RILU:
            if (dynamic_cast<const Tpetra::Experimental::BlockCrsMatrix<ST,LO,GO,NT>* >(mat.get())) {
                ifprec = factory.create<const Matrix>("RBILUK", mat);
            } else {
                ifprec = factory.create<const Matrix>("RILUK", mat);
            }
            params->set("fact: relax value", sb.getRelaxationFactor());
            // override if set explicitly for trilinos
            extractParamIfSet<ST>("fact: relax value", pyParams, *params);
            extractParamIfSet<int>("fact: iluk level-of-fill", pyParams, *params);
            extractParamIfSet<int>("fact: iluk level-of-overlap", pyParams, *params);
            extractParamIfSet<ST>("fact: absolute threshold", pyParams, *params);
            extractParamIfSet<ST>("fact: relative threshold", pyParams, *params);
            break;
        default:
            throw escript::ValueError("Unsupported preconditioner requested.");
    }
    if (!ifprec.is_null()) {
        ifprec->setParameters(*params);
        ifprec->initialize();
        ifprec->compute();
        prec = ifprec;
    }
    return prec;
}

// instantiate our two supported versions
typedef MatrixType<real_t> RealMatrix;
typedef MatrixType<cplx_t> ComplexMatrix;

template
RCP<RealOperator> createPreconditioner<real_t>(RCP<const RealMatrix> mat,
                                               const escript::SolverBuddy& sb);
template
RCP<ComplexOperator> createPreconditioner<cplx_t>(RCP<const ComplexMatrix> mat,
                                               const escript::SolverBuddy& sb);

}  // end of namespace