/*
 * The Conv integrators will implicitly determine the structure of the previous
 * layer, as they have to be structured in a specific way. The Net and reservoir
 * however, specify the connections either within or between layers.
 * Within a layer, a neighbor graph is used, where sources of the connections
 * correspond with nodes in the same graph.
 * Between layers, the sources of the connections correspond to the nodes
 * of the previous layer, while the destination corresponds with nodes in the
 * current layer.
 *
 * Integrators don't actually have to know the real size/shape of the previous
 * layer. Any size/shape can be given so long as the # elements is less than
 * then number in the previous layer so that non-existent elements are not
 * accessed. The integrator will create its own view into the previous layer
 * with the shape provided it, regardless of whether that layer has that shape.
 */

#ifndef NN_INTEGRATOR_H_
#define NN_INTEGRATOR_H_

#include <cstddef>
#include <vector>
#include <stdexcept>
#include <iostream>
#include <numeric>
#include <functional>
#include <utility>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include "../common/multi_array.hpp"
#include "../common/graphs.hpp"
#include "parameter_types.hpp"
#include "../common/utilities.hpp"

namespace nervous_system {

enum INTEGRATOR_TYPE {
  BASE_INTEGRATOR,
  NONE_INTEGRATOR,
  ALL2ALL_INTEGRATOR,
  CONV_INTEGRATOR,
  RECURRENT_INTEGRATOR,
  RESERVOIR_INTEGRATOR,
  RESERVOIR_HYBRID,
  TRUNCATED_RECURRENT_INTEGRATOR,
  CONV_EIGEN_INTEGRATOR,
  ALL2ALL_EIGEN_INTEGRATOR,
  RECURRENT_EIGEN_INTEGRATOR,
  RESERVOIR_EIGEN_INTEGRATOR,
  REWARD_MODULATED_INTEGRATOR,
  REWARD_MODULATED_ALL2ALL_INTEGRATOR,
  REWARD_MODULATED_RECURRENT_INTEGRATOR,
  REWARD_MODULATED_CONV_INTEGRATOR,
  REWARD_MODULATED
};

// Abstract base class
template<typename TReal>
class Integrator {
  public:
    typedef std::size_t Index;

    Integrator() {
      integrator_type_ = BASE_INTEGRATOR;
      parameter_count_ = 0;
    }
    virtual ~Integrator()=default;
    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                            multi_array::Tensor<TReal>& tar_state)=0;
    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters)=0;

    std::size_t GetParameterCount() const {
      return parameter_count_;
    }

    INTEGRATOR_TYPE GetIntegratorType() const {
      return integrator_type_;
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const=0;

    /*
     * Returns a pair representing the start and stop indices for the parameters
     * associated with link weights. These should always be contiguous.
     */
    virtual std::pair<Index, Index> GetWeightIndexRange() const=0;

  protected:
    INTEGRATOR_TYPE integrator_type_;
    std::size_t parameter_count_;
};

template <typename TReal>
class RewardModulatedIntegrator : public virtual Integrator<TReal>
{
  public:
    typedef std::size_t Index;
    typedef Integrator<TReal> super_type;

    RewardModulatedIntegrator(const TReal learning_rate)
        : super_type(), learning_rate_(learning_rate) {
    }
    virtual ~RewardModulatedIntegrator()= default;
    virtual void UpdateWeights(const TReal reward,
                               const TReal reward_average,
                               const multi_array::Tensor<TReal>& src_state,
                               const multi_array::Tensor<TReal>& tar_state,
                               const multi_array::Tensor<TReal>& tar_state_averages)=0;
    virtual const multi_array::Tensor<TReal>& GetWeights() const=0;

  protected:
    const TReal learning_rate_;
};

// None integrator - does nothing
template<typename TReal>
class NoneIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;

    NoneIntegrator() : super_type() { super_type::integrator_type_ = NONE_INTEGRATOR; }
    virtual ~NoneIntegrator()=default;

    virtual void operator()(const multi_array::Tensor<TReal>& src_state, multi_array::Tensor<TReal>& tar_state) {}
    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {}

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      return std::vector<PARAMETER_TYPE>(super_type::parameter_count_);
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, 0);
    }
};

// Integrator that has All2All connectivity with previous layer
template<typename TReal>
class All2AllIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;

    All2AllIntegrator(Index num_states, Index num_prev_states)
        : num_states_(num_states), num_prev_states_(num_prev_states) {
      super_type::parameter_count_ = num_states_ * num_prev_states_;
      super_type::integrator_type_ = ALL2ALL_INTEGRATOR;
    }

    virtual void operator()(const multi_array::Tensor<TReal>& src_state, multi_array::Tensor<TReal>& tar_state) {
      if (!((src_state.size() == num_prev_states_) && (tar_state.size() == num_states_))) {
        std::cerr << "src state size: " << src_state.size() << std::endl;
        std::cerr << "prev state size: " << num_prev_states_ << std::endl;
        std::cerr << "tar state size: " << tar_state.size() << std::endl;
        std::cerr << "state size: " << num_states_ << std::endl;
        throw std::invalid_argument("src state size and prev state size "
                                    "must be equal. tar state size and state"
                                    " size must be equal");
      }
      Index weight_id = 0;
      for (Index iii = 0; iii < tar_state.size(); ++iii) {
        TReal cumulative_sum = 0.0;
        for (Index jjj = 0; jjj < src_state.size(); ++jjj) {
          cumulative_sum += src_state[jjj] * weights_[weight_id];
          ++weight_id;
        }
        tar_state[iii] = utilities::BoundState(cumulative_sum);
      }
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (parameters.size() != super_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << super_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      weights_ = multi_array::ConstArraySlice<TReal>(parameters);
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, super_type::parameter_count_);
    }

  protected:
    Index num_states_;
    Index num_prev_states_;
    multi_array::ConstArraySlice<TReal> weights_;
};

/*
 * Conv integrator - uses implicit structure
 * Uses separable filters, so # params for a KxHxW kernel is K*(H+W)
 *
 * F = layer.shape[0]
 * H/W = filter.shape[1/2]
 * D = filter.shape[0]
 * filter_shape and layer_shape implicitly contain most the information for
 * the shape of the previous layer. Because this is a separable filter
 * the # of parameters for each filter is (W * D) + (H * D).
 * This convolution uses spatially and depthwise separable convolutions.
 * First F HxW convolutions are applied to each input dimension D.
 * Then F Dx1x1 filters are applied to the results.
 * Principle behind DxHxW + DxFx1x1 is that the same filter is used on all
 * layers, and then a 1x1 filter is used to do a weighted sum. This is
 * opposed to having a separate filer for each input channel
 * (each 1x1 needs D parameters, with a total of F 1x1 filters)
 */
template<typename TReal>
class Conv2DIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;

    Conv2DIntegrator(const multi_array::Array<Index,3>& filter_shape,
        const multi_array::Array<Index,3>& layer_shape,
        const multi_array::Array<Index,3>& prev_layer_shape, Index stride)
        : num_filters_(layer_shape[0]), layer_shape_(layer_shape),
        prev_layer_shape_(prev_layer_shape),
        filter_shape_(filter_shape), stride_(stride),
        filter_parameters_major_({num_filters_}),
        filter_parameters_minor_({num_filters_}),
        channel_weights_({num_filters_}) {

      min_src_size_ = std::accumulate(prev_layer_shape_.begin(),
        prev_layer_shape_.end(), 1, std::multiplies<TReal>());
      min_tar_size_ = std::accumulate(layer_shape_.begin(), layer_shape_.end(),
        1, std::multiplies<TReal>());
      // NumDim should be 3 for stride calculation
      multi_array::CalculateStrides(layer_shape_.data(), layer_strides_.data(), 3);
      multi_array::CalculateStrides(prev_layer_shape_.data(), prev_layer_strides_.data(), 3);
      if (filter_shape[0] != prev_layer_shape[0]) {
        std::cerr << "first filter shape: " << filter_shape[0] << std::endl;
        std::cerr << "first prev layer shape: " << prev_layer_shape[0] << std::endl;
        throw std::invalid_argument("First dimensions must be equal.");
      }

      super_type::parameter_count_ = num_filters_
        * (filter_shape_[2] + filter_shape_[1] + filter_shape_[0]);
      super_type::integrator_type_ = CONV_INTEGRATOR;
      firstpass_buffer_ = multi_array::Tensor<TReal>(
                            {prev_layer_shape[1], layer_shape[2]});
      secondpass_buffer_ = multi_array::Tensor<TReal>(
                            {layer_shape[1], layer_shape[2]});
    }
    virtual ~Conv2DIntegrator()=default;

    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                            multi_array::Tensor<TReal>& tar_state) {
      /*
       * tar_state needs to have AT LEAST as many elements as required by
       * layer_shape, but it doesn't not need to have the same shape.
       * tar_state will be re-interpreted as the same shape as layer_shape
       * src_state will be reinterpreted as the same shape as prev_layer_shape
       */
      if (src_state.size() < min_src_size_) {
        throw std::invalid_argument("Src state too small for integrator");
      }
      const multi_array::TensorView<TReal> src_view(src_state.data(),
        prev_layer_strides_.data(), prev_layer_shape_.data());

      if (tar_state.size() < min_tar_size_) {
        throw std::invalid_argument("tar state too small for integrator");
      }
      multi_array::TensorView<TReal> tar_view(tar_state.data(),
        layer_strides_.data(), layer_shape_.data());

      multi_array::ArrayView<multi_array::ConstArraySlice<TReal>, 1> major_view(filter_parameters_major_);
      multi_array::ArrayView<multi_array::ConstArraySlice<TReal>, 1> minor_view(filter_parameters_minor_);
      multi_array::ArrayView<multi_array::ConstArraySlice<TReal>, 1> channel_view(channel_weights_);

      // for each filter
      for (Index iii = 0; iii < num_filters_; iii++) {
        multi_array::TensorView<TReal> tar_image = tar_view[iii];
        // for each input channel
        for (Index jjj = 0; jjj < src_view.extent(0); jjj++) {
          // Carry out separable conv on layer of inputs
          const multi_array::TensorView<TReal> src_image = src_view[jjj];
          multi_array::TensorView<TReal> secondpass_image = secondpass_buffer_.accessor();
          multi_array::TensorView<TReal> firstpass_image = firstpass_buffer_.accessor();
          Convolve2D(src_image, secondpass_image, firstpass_image,
                     major_view[iii], minor_view[iii], stride_);

          for (Index kkk = 0; kkk < tar_view.extent(1); ++kkk) {
            for (Index lll = 0; lll < tar_view.extent(2); ++lll) {
              tar_image[kkk][lll] += secondpass_image[kkk][lll] * channel_weights_[iii][jjj];
              tar_image[kkk][lll] = utilities::BoundState<TReal>(tar_image[kkk][lll]);
            }
          }
        }
      }
    }

    void Convolve2D(const multi_array::TensorView<TReal>& src,
        multi_array::TensorView<TReal>& tar,
        multi_array::TensorView<TReal>& buffer,
        const multi_array::ConstArraySlice<TReal> &kernel_minor,
        const multi_array::ConstArraySlice<TReal> &kernel_major,
        Index stride) {

      // If stride is larger than the kernel, cumulative sum is reset
      if (stride < kernel_major.size()) {
        // Accumulate through major axis
        Index half_kernel = kernel_major.size() / 2;
        // Minor axis loop
        TReal cumulative_sum = 0.0;
        Index output_index = 0;
        for (Index iii = 0; iii < src.extent(0); ++iii) {
          cumulative_sum = 0.0;
          output_index = 0;
          // Initiate sum
          // Add out-of-bounds components
          for (Index kkk = 0; kkk < half_kernel; kkk++) {
            cumulative_sum += kernel_major[kkk] * src[iii][0];
          }
          // Add in-bounds components
          for (Index kkk = half_kernel; kkk < kernel_major.size(); kkk++) {
            cumulative_sum += kernel_major[kkk] * src[iii][kkk - half_kernel];
          }
          buffer[iii][output_index++] = cumulative_sum;

          // Roll sum (out-of-bounds components)
          Index jjj = stride;
          for (; jjj < half_kernel+stride; jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_major.size() - kkk - 1] * src[iii][jjj + half_kernel - kkk]
                              - kernel_major[kkk] * src[iii][0];
            }
            buffer[iii][output_index++] = cumulative_sum;
          }

          //  Roll sum (in-bounds components)
          for (; jjj < src.extent(1)-half_kernel; jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_major.size() - kkk - 1] * src[iii][jjj + half_kernel - kkk]
                              - kernel_major[kkk] * src[iii][jjj - half_kernel - 1 - kkk];
            }
            buffer[iii][output_index++] = cumulative_sum;
          }

          // Roll sum (out-of-bounds end components)
          for (; jjj < src.extent(1); jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_major.size() - kkk - 1] * src[iii][src.extent(1)-1]
                              - kernel_major[kkk] * src[iii][jjj - half_kernel - 1 - kkk];
            }
            buffer[iii][output_index++] = cumulative_sum;
          }
        }
      }
      else {
        // Accumulate through major axis
        Index half_kernel = kernel_major.size() / 2;
        // Minor axis loop
        TReal cumulative_sum = 0.0;
        Index output_index = 0;
        for (Index iii = 0; iii < src.extent(0); ++iii) {
          cumulative_sum = 0.0;
          output_index = 0;
          // Initiate sum
          // Add out-of-bounds components
          for (Index kkk = 0; kkk < half_kernel; kkk++) {
            cumulative_sum += kernel_major[kkk] * src[iii][0];
          }
          // Add in-bounds components
          for (Index kkk = half_kernel; kkk < kernel_major.size(); kkk++) {
            cumulative_sum += kernel_major[kkk] * src[iii][kkk - half_kernel];
          }
          buffer[iii][output_index++] = cumulative_sum;

          //  Roll sum (in-bounds components)
          Index jjj = stride;
          for (; jjj < src.extent(1)-half_kernel; jjj+=stride) {
            cumulative_sum = 0.0;
            for (Index kkk = 0; kkk < kernel_major.size(); ++kkk) {
              cumulative_sum += kernel_major[kkk] * src[iii][jjj - half_kernel + kkk];
            }
            buffer[iii][output_index++] = cumulative_sum;
          }

          // Roll sum (out-of-bounds end components)
          for (; jjj < src.extent(1); jjj+=stride) {
            cumulative_sum = 0.0;
            // Add in-bounds components
            for (Index kkk = 0; kkk < half_kernel+1; kkk++) {
              cumulative_sum += kernel_major[kkk] * src[iii][jjj - half_kernel + kkk];
            }
            // Add out-of-bounds components
            for (Index kkk = half_kernel+1; kkk < kernel_major.size(); kkk++) {
              cumulative_sum += kernel_major[kkk] * src[iii][src.extent(1)-1];
            }
            buffer[iii][output_index++] = cumulative_sum;
          }
        }
      }
      if (stride < kernel_minor.size()) {
        // Accumulate through minor axis
        TReal cumulative_sum = 0.0;
        Index output_index = 0;
        Index half_kernel = kernel_minor.size() / 2;
        for (Index iii = 0; iii < buffer.extent(1); iii++) {
          cumulative_sum = 0.0;
          output_index = 0;
          // Initiate sum
          // Add out-of-bounds components
          for (Index kkk = 0; kkk < half_kernel; kkk++) {
            cumulative_sum += kernel_minor[kkk] * buffer[0][iii];
          }
          // Add in-bounds components
          for (Index kkk = half_kernel; kkk < kernel_minor.size(); kkk++) {
            cumulative_sum += kernel_minor[kkk] * buffer[kkk - half_kernel][iii];
          }
          tar[output_index++][iii] = cumulative_sum;

          // Roll sum (out-of-bounds components)
          Index jjj = stride;
          for (; jjj < half_kernel+stride; jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_minor.size() - kkk - 1] * buffer[jjj + half_kernel - kkk][iii]
                              - kernel_minor[kkk] * buffer[0][iii];
            }
            tar[output_index++][iii] = cumulative_sum;
          }

          //  Roll sum (in-bounds components)
          for (; jjj < buffer.extent(0)-half_kernel; jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_minor.size() - kkk - 1] * buffer[jjj + half_kernel - kkk][iii]
                              - kernel_minor[kkk] * buffer[jjj - half_kernel - 1 - kkk][iii];
            }
            tar[output_index++][iii] = cumulative_sum;
          }

          // Roll sum (out-of-bounds end components)
          for (; jjj < buffer.extent(0); jjj+=stride) {
            for (Index kkk = 0; kkk < stride; ++kkk) {
              cumulative_sum += kernel_major[kernel_minor.size() - kkk - 1] * buffer[src.extent(0)-1][iii]
                              - kernel_minor[kkk] * buffer[jjj - half_kernel - 1 - kkk][iii];
            }
            tar[output_index++][iii] = cumulative_sum;
          }
        }
      }
      else {

        // Accumulate through major axis
        Index half_kernel = kernel_minor.size() / 2;
        TReal cumulative_sum = 0.0;
        Index output_index = 0;
        // Minor axis loop
        for (Index iii = 0; iii < buffer.extent(1); ++iii) {
          cumulative_sum = 0.0;
          output_index = 0;
          // Initiate sum
          // Add out-of-bounds components
          for (Index kkk = 0; kkk < half_kernel; kkk++) {
            cumulative_sum += kernel_minor[kkk] * buffer[0][iii];
          }
          // Add in-bounds components
          for (Index kkk = half_kernel; kkk < kernel_minor.size(); kkk++) {
            cumulative_sum += kernel_minor[kkk] * buffer[kkk - half_kernel][iii];
          }
          tar[output_index++][iii] = cumulative_sum;

          //  Roll sum (in-bounds components)
          Index jjj = stride;
          for (; jjj < buffer.extent(0)-half_kernel; jjj+=stride) {
            cumulative_sum = 0.0;
            for (Index kkk = 0; kkk < kernel_minor.size(); ++kkk) {
              cumulative_sum += kernel_minor[kkk] * buffer[jjj - half_kernel + kkk][iii];
            }
            tar[output_index++][iii] = cumulative_sum;
          }

          // Roll sum (out-of-bounds end components)
          for (; jjj < buffer.extent(0); jjj+=stride) {
            cumulative_sum = 0.0;
            // Add in-bounds components
            for (Index kkk = 0; kkk < half_kernel+1; kkk++) {
              cumulative_sum += kernel_minor[kkk] * buffer[jjj - half_kernel + kkk][iii];
            }
            // Add out-of-bounds components
            for (Index kkk = half_kernel+1; kkk < kernel_minor.size(); kkk++) {
              cumulative_sum += kernel_minor[kkk] * buffer[buffer.extent(0)-1][iii];
            }
            tar[output_index++][iii] = cumulative_sum;
          }
        }
      }
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {

      if (parameters.size() != super_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << super_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      multi_array::ArrayView< multi_array::ConstArraySlice<TReal>, 1> major_view(filter_parameters_major_);
      multi_array::ArrayView< multi_array::ConstArraySlice<TReal>, 1> minor_view(filter_parameters_minor_);
      multi_array::ArrayView< multi_array::ConstArraySlice<TReal>, 1> channel_view(channel_weights_);

      Index start(0);
      for (Index filter = 0; filter < num_filters_; filter++) {
        major_view[filter] = parameters.slice(start, filter_shape_[2]);
        start += parameters.stride() * filter_shape_[2];

        minor_view[filter] = parameters.slice(start, filter_shape_[1]);
        start += parameters.stride() * filter_shape_[1];

        channel_view[filter] = parameters.slice(start, filter_shape_[0]);
        start += parameters.stride() * filter_shape_[0];
      }
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, super_type::parameter_count_);
    }

    const multi_array::Array<Index, 3>& GetFilterShape() const {
      return filter_shape_;
    };

    Index GetMinTarSize() const {
      return min_tar_size_;
    }

  protected:
    Index num_filters_;
    multi_array::Array<Index, 3> layer_shape_;
    multi_array::Array<Index, 3> prev_layer_shape_;
    multi_array::Array<Index, 3> filter_shape_;
    Index stride_;
    Index min_src_size_;
    Index min_tar_size_;
    multi_array::Array<Index, 3> prev_layer_strides_;
    multi_array::Array<Index, 3> layer_strides_;
    multi_array::MultiArray<multi_array::ConstArraySlice<TReal>, 1> filter_parameters_major_;
    multi_array::MultiArray<multi_array::ConstArraySlice<TReal>, 1> filter_parameters_minor_;
    multi_array::MultiArray<multi_array::ConstArraySlice<TReal>, 1> channel_weights_;
    multi_array::Tensor<TReal> secondpass_buffer_;
    multi_array::Tensor<TReal> firstpass_buffer_;
};

// Network integrator -- uses explicit unweighted structure
template<typename TReal>
class RecurrentIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;

    RecurrentIntegrator(const graphs::PredecessorGraph<>& network)
        : network_(network) {
      super_type::integrator_type_ = RECURRENT_INTEGRATOR;
      super_type::parameter_count_ = network_.NumEdges();
    }

    virtual ~RecurrentIntegrator()=default;

    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) {

      /*
       * Graph maybe a connector graph or an internal graph.
       * In case of connector, the num nodes doesn't need to match tar_state,
       * because predecessors should be empty for nodes not in tar_state.
       * However, src state does have to be checked using (at) when tar_state
       * is larger than src_state, to ensure nothing invalid is accessed.
       */
      Index edge_id = 0;
      for (Index node = 0; node < network_.NumNodes(); ++node) {
        for (Index iii = 0; iii < network_.Predecessors(node).size(); ++iii) {
          tar_state.at(node) += src_state.at(network_.Predecessors(node)[iii].source) * weights_[edge_id];
          tar_state[node] = utilities::BoundState(tar_state[node]);
          ++edge_id;
        }
      }
      if (edge_id != network_.NumEdges()) {
        throw std::runtime_error("Miss match between number of edges and the"
                                 " number integrated");
      }
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (parameters.size() != super_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << super_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      weights_ = parameters.slice(0, super_type::parameter_count_);
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    const multi_array::ConstArraySlice<TReal>& GetWeights() const {
      return weights_;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, super_type::parameter_count_);
    }

    const graphs::PredecessorGraph<>& GetGraph() const {
      return network_;
    }

  protected:
    graphs::PredecessorGraph<> network_;
    multi_array::ConstArraySlice<TReal> weights_;
};

// Truncated Recurrent integrator sets weights to 0 during calculations if
// they are below the magnitude of the threshold.
template<typename TReal>
class TruncatedRecurrentIntegrator : public virtual RecurrentIntegrator<TReal> {
  public:
    typedef RecurrentIntegrator<TReal> super_type;
    typedef typename super_type::Index Index;

    TruncatedRecurrentIntegrator(const graphs::PredecessorGraph<>& network,
                                 TReal weight_threshold)
        : super_type(network), weight_threshold_(weight_threshold) {

      super_type::integrator_type_ = TRUNCATED_RECURRENT_INTEGRATOR;
    }

    virtual ~TruncatedRecurrentIntegrator()= default;

    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) {

      Index edge_id = 0;
      for (Index node = 0; node < super_type::network_.NumNodes(); ++node) {
        for (Index iii = 0; iii < super_type::network_.Predecessors(node).size(); ++iii) {
          // Only carry out calculation if it exceeds magnitude of the threshold
          if ((super_type::weights_[edge_id] > weight_threshold_
               && super_type::weights_[edge_id] >= 0) ||
              (super_type::weights_[edge_id] < -weight_threshold_
               && super_type::weights_[edge_id] <= 0)) {
            tar_state.at(node) += src_state.at(super_type::network_.Predecessors(node)[iii].source)
                               * super_type::weights_[edge_id];
            tar_state[node] = utilities::BoundState(tar_state[node]);
          }
          ++edge_id;
        }
      }
      if (edge_id != super_type::network_.NumEdges()) {
        throw std::runtime_error("Miss match between number of edges and the"
                                 " number integrated");
      }
    }

    TReal GetWeightThreshold() const {
      return weight_threshold_;
    }

  protected:
    TReal weight_threshold_;
};

// Reservoir -- uses explicit weighted structure
template<typename TReal>
class ReservoirIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;

    ReservoirIntegrator(const graphs::PredecessorGraph<TReal>& network)
        : network_(network) {
      super_type::integrator_type_ = RESERVOIR_INTEGRATOR;
      super_type::parameter_count_ = 0;
    }

    ~ReservoirIntegrator()=default;

    void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) {

      for (Index node = 0; node < network_.NumNodes(); ++node) {
        for (Index iii = 0; iii < network_.Predecessors(node).size(); ++iii) {
          tar_state.at(node) += src_state.at(network_.Predecessors(node)[iii].source)
                              * network_.Predecessors(node)[iii].weight;
          tar_state[node] = utilities::BoundState(tar_state[node]);
        }
      }
    }

    void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {}

    std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      return std::vector<PARAMETER_TYPE>(super_type::parameter_count_);
    }

    std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, 0);
    }

    const graphs::PredecessorGraph<TReal>& GetGraph() const {
      return network_;
    }

  protected:
    graphs::PredecessorGraph<TReal> network_;
};

/*
 * Implements non-separable convolution using im2col and gemm through Eigen
 * Assumes NCHW memory layout
 * TODO: Support even filters
 */
template<typename TReal>
class ConvEigenIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef Eigen::Matrix<TReal, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    typedef const Eigen::Matrix<TReal, Eigen::Dynamic, Eigen::Dynamic> ConstMatrix;
    typedef Eigen::Map<Matrix> MatrixView;
    typedef const Eigen::Map<ConstMatrix> ConstMatrixView;

    /*
     * Filter shape order: {depth, height, width}
     * Layer shape order: {# channels, height, width}
     * Prev Layer order: {# channels, height, width}
     * Note: This is a bit confusing because my tensors us C++ memory order
     * and Eigen uses Fortran. So I list shapes here with last element of shape
     * being the major axis. This is reversed in Eigen, where first is the major.
     */
    ConvEigenIntegrator(const multi_array::Array<Index,3>& filter_shape,
                        const multi_array::Array<Index,3>& layer_shape,
                        const multi_array::Array<Index,3>& prev_layer_shape,
                        Index stride)
                      : num_filters_(layer_shape[0]), layer_shape_(layer_shape),
                        prev_layer_shape_(prev_layer_shape),
                        filter_shape_(filter_shape), stride_(stride),
                        channels_(prev_layer_shape[0]),
                        height_(prev_layer_shape[1]),
                        width_(prev_layer_shape[2]),
                        kernel_h_(filter_shape[1]),
                        kernel_w_(filter_shape[2]),
                        pad_h_(kernel_h_/2),
                        pad_w_(kernel_w_/2),
                        channel_size_(((height_ + 2 * pad_h_ - kernel_h_)
                                       / stride_ + 1)
                                      * ((width_ + 2 * pad_w_ - kernel_w_)
                                         / stride_ + 1)),
                        buffer_state_(channel_size_,
                                      kernel_h_ * kernel_w_ * channels_) {
      super_type::parameter_count_ = kernel_w_ * kernel_h_ * channels_ * num_filters_;
      super_type::integrator_type_ = CONV_EIGEN_INTEGRATOR;
    }

    virtual ~ConvEigenIntegrator()=default;

    /*
     * Matrix: (# rows, # cols) -> column major
     * src shape: {height * width, channels}
     * tar shape: {(new height * new width), num_filters}
     * Note: If you check tensor shape it will be the reverse, since I use C's
     * order, while Eigen uses Fortran's. Sorry for the confusion, but Eigen
     * got added in latter and I am not sure why they choose Fortran's way for a
     * C++ API.
     */
    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) override {

      utilities::Im2Col(src_state.data(), channels_, height_, width_,
                        kernel_h_, kernel_w_, pad_h_, pad_w_, stride_, stride_,
                        1, 1, buffer_state_.data());
      MatrixView output(tar_state.data(), channel_size_, num_filters_);
      ConstMatrixView params(weight_view_.data() + weight_view_.start(),
                             kernel_w_ * kernel_h_ * channels_,
                             num_filters_);
      output.noalias() = buffer_state_ * params;
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) override {
      weight_view_ = parameters;
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const override {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const override {
      return std::make_pair(0, super_type::parameter_count_);
    };

  protected:
    const utilities::Integer num_filters_;
    const multi_array::Array<Index, 3> layer_shape_;
    const multi_array::Array<Index, 3> prev_layer_shape_;
    const multi_array::Array<Index, 3> filter_shape_;
    const utilities::Integer stride_;
    const utilities::Integer channels_;
    const utilities::Integer height_;
    const utilities::Integer width_;
    const utilities::Integer kernel_h_;
    const utilities::Integer kernel_w_;
    const utilities::Integer pad_h_;
    const utilities::Integer pad_w_;
    const utilities::Integer channel_size_;
    Matrix buffer_state_;
    multi_array::ConstArraySlice<TReal> weight_view_;
};

template <typename TReal>
class RewardModulatedConvIntegrator : public ConvEigenIntegrator<TReal>,
                                      public RewardModulatedIntegrator<TReal> {
  public:
    typedef RewardModulatedIntegrator<TReal> reward_modulator_type;
    typedef ConvEigenIntegrator<TReal> conv_type;
    typedef typename conv_type::Index Index;
    typedef typename conv_type::Matrix Matrix;
    typedef typename conv_type::ConstMatrix ConstMatrix;
    typedef typename conv_type::MatrixView MatrixView;
    typedef typename conv_type::ConstMatrixView ConstMatrixView;

    RewardModulatedConvIntegrator(const multi_array::Array<Index,3>& filter_shape,
                                  const multi_array::Array<Index,3>& layer_shape,
                                  const multi_array::Array<Index,3>& prev_layer_shape,
                                  Index stride, const TReal learning_rate)
        : conv_type(filter_shape, layer_shape, prev_layer_shape, stride),
          reward_modulator_type(learning_rate),
          weights_({conv_type::parameter_count_}) {
      conv_type::integrator_type_ = REWARD_MODULATED;
    }

    void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) override {

      utilities::Im2Col(src_state.data(), conv_type::channels_,
                        conv_type::height_, conv_type::width_,
                        conv_type::kernel_h_, conv_type::kernel_w_,
                        conv_type::pad_h_, conv_type::pad_w_,
                        conv_type::stride_, conv_type::stride_,
                        1, 1, conv_type::buffer_state_.data());
      MatrixView output(tar_state.data(), conv_type::channel_size_,
                        conv_type::num_filters_);
      ConstMatrixView params(weights_.data(),
                             conv_type::kernel_w_
                             * conv_type::kernel_h_
                             * conv_type::channels_,
                             conv_type::num_filters_);
      output.noalias() = conv_type::buffer_state_ * params;
    }

    void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (parameters.size() != conv_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << conv_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }

      for (Index iii = 0; iii < parameters.size(); ++iii) {
        weights_[iii] = parameters[iii];
      }
      conv_type::weight_view_ = multi_array::ConstArraySlice<TReal>(weights_.data(), 0,
                                                         weights_.size());
    }

    void UpdateWeights(const TReal reward,
                       const TReal reward_average,
                       const multi_array::Tensor<TReal>& src_state,
                       const multi_array::Tensor<TReal>& tar_state,
                       const multi_array::Tensor<TReal>& tar_state_averages) {

      MatrixView weights(weights_.data(),
                         conv_type::kernel_w_
                         * conv_type::kernel_h_
                         * conv_type::channels_,
                         conv_type::num_filters_);
      ConstMatrixView tar_view(tar_state.data(), conv_type::channel_size_,
                               conv_type::num_filters_);
      ConstMatrixView tar_avg_view(tar_state_averages.data(),
                                   conv_type::channel_size_,
                                   conv_type::num_filters_);

      std::cout << "weights dim: cols " << weights.cols() << " rows " << weights.rows() << std::endl;/////////////
      std::cout << "tar view dim: cols " << tar_view.cols() << " rows " << tar_view.rows() << std::endl;/////////////
      std::cout << "tar avg view dim: cols " << tar_avg_view.cols() << " rows " << tar_avg_view.rows() << std::endl;/////////////
      std::cout << "buff dim: cols " << conv_type::buffer_state_.cols() << " rows " << conv_type::buffer_state_.rows() << std::endl;/////////////

      // find max index tar state, updates single filter
      const Index max_neuron_index = utilities::IndexOfMaxElement(tar_state);
      // Get the filter index for the max neuron
      const Index max_filter = max_neuron_index / conv_type::channel_size_;
      // The buffer_state im2col matrix has a window of states from the previous
      // layer that will correspond to the states that need to be integrated
      // for a given position in the current layer (same for each channel)
      // This position is modulo the size of the channel:
      const Index max_neuron_window = max_neuron_index % conv_type::channel_size_;
      // Alternatively find max index tar state for each filter, updates each filter

      // Loops through src neuron states
      const TReal reward_modulated_learning_factor = (reward - reward_average)
                                                     * reward_modulator_type::learning_rate_;
      for (auto src_neuron = 0; src_neuron < conv_type::buffer_state_.cols(); ++src_neuron) {
        weights(src_neuron, max_filter) += conv_type::buffer_state_(max_neuron_window,
                                                                    src_neuron)
                                 * (tar_state[max_neuron_index]
                                    - tar_state_averages[max_neuron_index])
                                 * reward_modulated_learning_factor;
      }
    }

    const multi_array::Tensor<TReal>& GetWeights() const
    {
      return weights_;
    }

  protected:
    multi_array::Tensor<TReal> weights_;
};

/*
 * Implements an All2All integrator with an Eigen backend
 */
template<typename TReal>
class All2AllEigenIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef Eigen::Matrix<TReal, Eigen::Dynamic, 1> ColVector;
    typedef Eigen::Map<ColVector> ColVectorView;
    typedef const Eigen::Matrix<TReal, Eigen::Dynamic, 1> ConstColVector;
    typedef const Eigen::Map<ConstColVector> ConstColVectorView;
    typedef const Eigen::Matrix<TReal, Eigen::Dynamic, Eigen::Dynamic> ConstMatrix;
    typedef const Eigen::Map<ConstMatrix> ConstMatrixView;

    All2AllEigenIntegrator(Index num_states, Index num_prev_states)
    : num_states_(num_states), num_prev_states_(num_prev_states) {
      super_type::parameter_count_ = num_states_ * num_prev_states_;
      super_type::integrator_type_ = ALL2ALL_EIGEN_INTEGRATOR;
    }

    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                            multi_array::Tensor<TReal>& tar_state) {
      if (!((src_state.size() == num_prev_states_) && (tar_state.size() == num_states_))) {
        std::cerr << "src state size: " << src_state.size() << std::endl;
        std::cerr << "prev state size: " << num_prev_states_ << std::endl;
        std::cerr << "tar state size: " << tar_state.size() << std::endl;
        std::cerr << "state size: " << num_states_ << std::endl;
        throw std::invalid_argument("src state size and prev state size "
                                    "must be equal. tar state size and state"
                                    " size must be equal");
      }

      ColVectorView output(tar_state.data(), tar_state.size());
      output.noalias() = ConstMatrixView(weight_view_.data() + weight_view_.start(),
                                         tar_state.size(),
                                         src_state.size())
                         * ConstColVectorView(src_state.data(), src_state.size());
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (parameters.size() != super_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << super_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      weight_view_ = parameters;
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, super_type::parameter_count_);
    }

  protected:
    Index num_states_;
    Index num_prev_states_;
    multi_array::ConstArraySlice<TReal> weight_view_;
};

/*
 * Implements a recurrent integrator with Eigen
 */
template<typename TReal>
class RecurrentEigenIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef Eigen::Matrix<TReal, Eigen::Dynamic, 1> ColVector;
    typedef Eigen::Map<ColVector> ColVectorView;
    typedef const Eigen::Matrix<TReal, Eigen::Dynamic, 1> ConstColVector;
    typedef const Eigen::Map<ConstColVector> ConstColVectorView;
    typedef Eigen::SparseMatrix<TReal> SparseMatrix;
    typedef const Eigen::SparseMatrix<TReal> ConstSparseMatrix;
    typedef const Eigen::Map<ConstSparseMatrix> ConstSparseMatrixView;

    RecurrentEigenIntegrator(SparseMatrix network) : network_(std::move(network)) {
      network_.makeCompressed();
      super_type::integrator_type_ = RECURRENT_EIGEN_INTEGRATOR;
      super_type::parameter_count_ = network_.nonZeros();
    }

    virtual ~RecurrentEigenIntegrator()=default;

    virtual void operator()(const multi_array::Tensor<TReal>& src_state,
                            multi_array::Tensor<TReal>& tar_state) {

      if ((network_.cols() != src_state.size())
          && (network_.rows() != tar_state.size())) {
        throw std::invalid_argument("src state size and tar state size "
                                    "incompatible with network");
      }

      ConstSparseMatrixView weight_matrix(network_.rows(), network_.cols(),
                                          weight_view_.size(), network_.outerIndexPtr(),
                                          network_.innerIndexPtr(),
                                          weight_view_.data() + weight_view_.start(),
                                          network_.innerNonZeroPtr());
      ColVectorView output_vector(tar_state.data(), tar_state.size());
      ConstColVectorView src_vector(src_state.data(), src_state.size());

      output_vector.noalias() = weight_matrix * src_vector;
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (parameters.size() != super_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << super_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      weight_view_ = parameters.slice(0, super_type::parameter_count_);
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);
      for (Index iii = 0; iii < super_type::parameter_count_; ++iii) {
        layout[iii] = WEIGHT;
      }
      return layout;
    }

    const multi_array::ConstArraySlice<TReal>& GetWeights() const {
      return weight_view_;
    }

    virtual std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, super_type::parameter_count_);
    }

  protected:
    SparseMatrix network_;
    multi_array::ConstArraySlice<TReal> weight_view_;
};

/*
 * Implements a reservior layer using Eigen
 */
template<typename TReal>
class ReservoirEigenIntegrator : public virtual Integrator<TReal> {
  public:
    typedef Integrator<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef Eigen::Matrix<TReal, Eigen::Dynamic, 1> ColVector;
    typedef Eigen::Map <ColVector> ColVectorView;
    typedef const Eigen::Matrix<TReal, Eigen::Dynamic, 1> ConstColVector;
    typedef const Eigen::Map <ConstColVector> ConstColVectorView;
    typedef Eigen::SparseMatrix <TReal> SparseMatrix;

    ReservoirEigenIntegrator(SparseMatrix network)
    : network_(std::move(network)) {
      network_.makeCompressed();
      super_type::integrator_type_ = RESERVOIR_EIGEN_INTEGRATOR;
      super_type::parameter_count_ = 0;
    }

    ~ReservoirEigenIntegrator()=default;

    void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) {

      if ((network_.cols() != src_state.size())
          && (network_.rows() != tar_state.size())) {
        throw std::invalid_argument("src state size and tar state size "
                                    "incompatible with network");
      }

      ColVectorView output_vector(tar_state.data(), tar_state.size());
      ConstColVectorView src_vector(src_state.data(), src_state.size());
      output_vector.noalias() = network_ * src_vector;
    }

    void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {}

    std::vector<PARAMETER_TYPE> GetParameterLayout() const {
      return std::vector<PARAMETER_TYPE>(super_type::parameter_count_);
    }

    std::pair<Index, Index> GetWeightIndexRange() const {
      return std::make_pair(0, 0);
    }

  protected:
    SparseMatrix network_;
};

template <typename TReal>
class RewardModulatedAll2AllIntegrator : public All2AllEigenIntegrator<TReal>,
                                         public RewardModulatedIntegrator<TReal> {
  public:
    typedef All2AllEigenIntegrator<TReal> all2all_type;
    typedef RewardModulatedIntegrator<TReal> reward_modulator_type;
    typedef typename all2all_type::Index Index;
    typedef typename all2all_type::ColVector ColVector;
    typedef typename all2all_type::ColVectorView ColVectorView;
    typedef typename all2all_type::ConstColVector ConstColVector;
    typedef typename all2all_type::ConstColVectorView ConstColVectorView;
    typedef typename all2all_type::ConstMatrix ConstMatrix;
    typedef typename all2all_type::ConstMatrixView ConstMatrixView;
    typedef Eigen::Matrix<TReal, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    typedef Eigen::Map<Matrix> MatrixView;

    RewardModulatedAll2AllIntegrator(const Index num_states,
                                     const Index num_prev_states,
                                     const TReal learning_rate)
      : all2all_type(num_states, num_prev_states),
        reward_modulator_type(learning_rate),
        weights_({all2all_type::parameter_count_}) {
      all2all_type::integrator_type_ = REWARD_MODULATED;
    }

    void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) override
    {
      if (!((src_state.size() == all2all_type::num_prev_states_)
            && (tar_state.size() == all2all_type::num_states_))) {
        std::cerr << "src state size: " << src_state.size() << std::endl;
        std::cerr << "prev state size: " << all2all_type::num_prev_states_ << std::endl;
        std::cerr << "tar state size: " << tar_state.size() << std::endl;
        std::cerr << "state size: " << all2all_type::num_states_ << std::endl;
        throw std::invalid_argument("src state size and prev state size "
                                    "must be equal. tar state size and state"
                                    " size must be equal");
      }

      ColVectorView output(tar_state.data(), tar_state.size());
      output.noalias() = ConstMatrixView(weights_.data(),
                                         tar_state.size(),
                                         src_state.size())
                         * ConstColVectorView(src_state.data(), src_state.size());
    }

    void Configure(const multi_array::ConstArraySlice<TReal>& parameters) override {
      if (parameters.size() != all2all_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << all2all_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }
      for (Index iii = 0; iii < parameters.size(); ++iii) {
        weights_[iii] = parameters[iii];
      }
      all2all_type::weight_view_ = multi_array::ConstArraySlice<TReal>(weights_.data(), 0,
                                                         weights_.size());
    }

    void UpdateWeights(const TReal reward,
                       const TReal reward_average,
                       const multi_array::Tensor<TReal>& src_state,
                       const multi_array::Tensor<TReal>& tar_state,
                       const multi_array::Tensor<TReal>& tar_state_averages) override {

      ConstColVectorView src_view(src_state.data(), src_state.size());
      ConstColVectorView tar_view(tar_state.data(), tar_state.size());
      ConstColVectorView tar_avg_view(tar_state_averages.data(), tar_state_averages.size());
      MatrixView weight_view(weights_.data(), tar_state.size(), src_state.size());
      const TReal reward_modulated_learning_factor = (reward - reward_average)
                                                   * reward_modulator_type::learning_rate_;
      for (Index s = 0; s < src_state.size(); ++s) {
        for (Index t = 0; t < tar_state.size(); ++t) {
          weight_view(t, s) += src_view(s)
                               * (tar_view(t) - tar_avg_view(t))
                               * reward_modulated_learning_factor;
        }
      }
    }

    const multi_array::Tensor<TReal>& GetWeights() const
    {
      return weights_;
    }

  protected:
    multi_array::Tensor<TReal> weights_;
};

template <typename TReal>
class RewardModulatedRecurrentIntegrator : public RecurrentEigenIntegrator<TReal>,
                                           public RewardModulatedIntegrator<TReal> {
  public:
    typedef RecurrentEigenIntegrator<TReal> recurrent_type;
    typedef RewardModulatedIntegrator<TReal> reward_modulator_type;
    typedef typename recurrent_type::Index Index;
    typedef typename recurrent_type::ColVector ColVector;
    typedef typename recurrent_type::ColVectorView ColVectorView;
    typedef typename recurrent_type::ConstColVector ConstColVector;
    typedef typename recurrent_type::ConstColVectorView ConstColVectorView;
    typedef typename recurrent_type::SparseMatrix SparseMatrix;
    typedef typename Eigen::Map<SparseMatrix> SparseMatrixView;
    typedef typename recurrent_type::ConstSparseMatrix ConstSparseMatrix;
    typedef typename recurrent_type::ConstSparseMatrixView ConstSparseMatrixView;

    RewardModulatedRecurrentIntegrator(SparseMatrix network, const TReal learning_rate)
        : recurrent_type(network), reward_modulator_type(learning_rate),
          weights_({recurrent_type::parameter_count_}) {
      recurrent_type::integrator_type_ = REWARD_MODULATED;
    }

    void operator()(const multi_array::Tensor<TReal>& src_state,
                    multi_array::Tensor<TReal>& tar_state) override {

      if ((recurrent_type::network_.cols() != src_state.size())
          && (recurrent_type::network_.rows() != tar_state.size())) {
        throw std::invalid_argument("src state size and tar state size "
                                    "incompatible with network");
      }

      ConstSparseMatrixView weight_matrix(recurrent_type::network_.rows(),
                                          recurrent_type::network_.cols(),
                                          weights_.size(),
                                          recurrent_type::network_.outerIndexPtr(),
                                          recurrent_type::network_.innerIndexPtr(),
                                          weights_.data(),
                                          recurrent_type::network_.innerNonZeroPtr());
      ColVectorView output_vector(tar_state.data(), tar_state.size());
      ConstColVectorView src_vector(src_state.data(), src_state.size());

      output_vector.noalias() = weight_matrix * src_vector;
    }

    void Configure(const multi_array::ConstArraySlice<TReal>& parameters) override {
      if (parameters.size() != recurrent_type::parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << recurrent_type::parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters");
      }

      for (Index iii = 0; iii < parameters.size(); ++iii) {
        weights_[iii] = parameters[iii];
      }
      recurrent_type::weight_view_ = multi_array::ConstArraySlice<TReal>(weights_.data(), 0,
                                                         weights_.size());
    }

    void UpdateWeights(const TReal reward,
                       const TReal reward_average,
                       const multi_array::Tensor<TReal>& src_state,
                       const multi_array::Tensor<TReal>& tar_state,
                       const multi_array::Tensor<TReal>& tar_state_averages) override {

      SparseMatrixView weight_matrix(recurrent_type::network_.rows(),
                                     recurrent_type::network_.cols(),
                                     weights_.size(),
                                     recurrent_type::network_.outerIndexPtr(),
                                     recurrent_type::network_.innerIndexPtr(),
                                     weights_.data(),
                                     recurrent_type::network_.innerNonZeroPtr());
      ConstColVectorView src_view(src_state.data(), src_state.size());
      ConstColVectorView tar_view(tar_state.data(), tar_state.size());
      ConstColVectorView tar_avg_view(tar_state_averages.data(), tar_state_averages.size());
      const TReal reward_modulated_learning_factor = (reward - reward_average)
                                                     * reward_modulator_type::learning_rate_;
      for (Index s = 0; s < weight_matrix.outerSize(); ++s) {
        for (typename SparseMatrixView::InnerIterator it(weight_matrix, s); it; ++it) {
          it.valueRef() += src_view(s)
                           * (tar_view(it.row()) - tar_avg_view(it.row()))
                           * reward_modulated_learning_factor;
        }
      }
    }

    const multi_array::Tensor<TReal>& GetWeights() const override
    {
      return weights_;
    }

  protected:
    multi_array::Tensor<TReal> weights_;
};

} // End nervous_system namespace

#endif /* NN_INTEGRATOR_H_ */


//
// Copyright (c) 2021 INRIA
//

#include <iostream>

#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/parallel/geometry.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

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

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_pool)
{
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");

  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
  Data data(model);

  const std::string package_path = PINOCCHIO_MODEL_DIR;
  hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
  std::vector<std::string> package_paths(1,package_path);
  pinocchio::GeometryModel geometry_model;
  pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);

  const int num_thread = omp_get_max_threads();
  GeometryPool pool(model,pinocchio::GeometryModel(),num_thread);

  pool.update(geometry_model);
  BOOST_CHECK(pool.geometry_model() == geometry_model);
  pool.update(GeometryData(geometry_model));

  pool.update(geometry_model,GeometryData(geometry_model));
}

BOOST_AUTO_TEST_CASE(test_talos)
{
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");

  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
  Data data(model), data_ref(model);

  const std::string package_path = PINOCCHIO_MODEL_DIR;
  hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
  std::vector<std::string> package_paths(1,package_path);
  pinocchio::GeometryModel geometry_model;
  pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);

  geometry_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);

  GeometryData geometry_data(geometry_model), geometry_data_ref(geometry_model);

  const Eigen::VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
  Eigen::VectorXd q = randomConfiguration(model,-qmax,qmax);

  const bool res_ref = computeCollisions(model,data_ref,geometry_model,geometry_data_ref,q);
  const bool res = computeCollisions(model,data,geometry_model,geometry_data,q);

  BOOST_CHECK(res_ref == res);
  BOOST_CHECK(geometry_data_ref.collisionPairIndex == geometry_data.collisionPairIndex);

  for(size_t k = 0; k < geometry_model.collisionPairs.size(); ++k)
  {
    const CollisionPair & cp = geometry_model.collisionPairs[k];
    BOOST_CHECK(geometry_data_ref.oMg[cp.first] == geometry_data.oMg[cp.first]);
    BOOST_CHECK(geometry_data_ref.oMg[cp.second] == geometry_data.oMg[cp.second]);
    BOOST_CHECK(   geometry_data_ref.collisionResults[k].getContacts()
                == geometry_data.collisionResults[k].getContacts());
  }
}

BOOST_AUTO_TEST_CASE(test_pool_talos)
{
  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");

  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
  Data data_ref(model);

  const std::string package_path = PINOCCHIO_MODEL_DIR;
  hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
  std::vector<std::string> package_paths(1,package_path);
  pinocchio::GeometryModel geometry_model;
  pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);

  geometry_model.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);

  GeometryData geometry_data_ref(geometry_model);

  const Eigen::VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
  const Eigen::DenseIndex batch_size = 128;
  const int num_thread = omp_get_max_threads();

  Eigen::MatrixXd q(model.nq,batch_size);
  for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
  {
    q.col(i) = randomConfiguration(model,-qmax,qmax);
  }

  typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
  VectorXb res(batch_size); res.fill(false);
  VectorXb res_ref(batch_size); res_ref.fill(false);
  for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
  {
    res_ref[i] = computeCollisions(model,data_ref,geometry_model,geometry_data_ref,q.col(i));
  }

  GeometryPool pool(model,geometry_model,num_thread);
  computeCollisions(num_thread,pool,q,res);

  BOOST_CHECK(res == res_ref);
}

BOOST_AUTO_TEST_SUITE_END()


//==========================================================================
//  NETBUILDER.CC - part of
//
//                     OMNeT++/OMNEST
//            Discrete System Simulation in C++
//
//==========================================================================

/*--------------------------------------------------------------*
  Copyright (C) 1992-2015 Andras Varga

  This file is distributed WITHOUT ANY WARRANTY. See the file
  `license' for details on this and other legal matters.
*--------------------------------------------------------------*/

#include <iostream>
#include <fstream>
#include <vector>
#include <string.h>
#include <omnetpp.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include "control_m.h"
#include "../utils/routing_table.hpp"
#include "../utils/traffic_matrix.hpp"

using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS> Graph;
typedef std::vector<std::pair<std::pair<int, int>, int>> RouterTable;

enum
{
    INIT = 0,
    BUILD_NETWORK = FSM_Transient(1),
    DISTRIBUTE_ADDRESS_GATE_MAP = FSM_Transient(2),
    WAITING_FOR_RESPONSE_GATE_MAP = FSM_Steady(1),
    DISTRIBUTE_ROUTING = FSM_Transient(3),
    WAITING_FOR_RESPONSE_ROUTING = FSM_Steady(2),
    DISTRIBUTE_TRAFFIC_MATRIX = FSM_Transient(4),
    WAITING_FOR_RESPONSE_TRAFFIC = FSM_Steady(3),
    FINISH = FSM_Steady(4)
};

using namespace omnetpp;

Control *generateControlPacket(const char *name, std::string command)
{
    Control *controlPacket = new Control(name);
    controlPacket->setCommand(command.c_str());
    return controlPacket;
}

/**
 * Builds a network dynamically, with the topology coming from a
 * text file.
 */
class NetBuilder : public cSimpleModule
{
protected:
    void connect(cGate *src, cGate *dest, double delay, double ber, double datarate);
    void buildNetwork(cModule *parent);
    void generateRandomTrafficMatrix(int trafficFactor);
    void sendAddressGateMap();
    void sendRoutingTable();
    void sendTrafficMatrix();
    virtual void initialize() override;
    virtual void handleMessage(cMessage *msg) override;

private:
    cFSM fsm;
    Graph *topo;

    int seed;
    int trafficFactor;

    boost::random::mt19937 rng;

    std::map<long, cModule *> nodeid2mod;
    std::map<long, std::map<int, int>> addressGateTableList;
    NetworkRoutingTable routingTable;
    TrafficMatrix trafficMatrix;
    int receiving_count;
};

Define_Module(NetBuilder);

void NetBuilder::initialize()
{
    // build the network in event 1, because it is undefined whether the simkernel
    // will implicitly initialize modules created *during* initialization, or this needs
    // to be done manually.
    scheduleAt(0, new cMessage());
    topo = nullptr;
    this->fsm.setName("builder_fsm");
    this->seed = par("seed").intValue();
    this->trafficFactor = par("trafficFactor").intValue();

    this->rng.seed(this->seed);
    this->trafficMatrix = TrafficMatrix(this->seed);

    EV << "random seed of NetBuilder is " << this->seed << std::endl;
    EV << "traffic factor of NetBuilder is " << this->trafficFactor << std::endl;
}

void NetBuilder::handleMessage(cMessage *msg)
{
    FSM_Switch(fsm)
    {
    case FSM_Exit(INIT):
        FSM_Goto(fsm, BUILD_NETWORK);
        break;

    case FSM_Exit(BUILD_NETWORK):
        if (!msg->isSelfMessage())
            throw cRuntimeError("This module does not process messages.");
        buildNetwork(getParentModule());
        FSM_Goto(fsm, DISTRIBUTE_ADDRESS_GATE_MAP);
        break;

    case FSM_Exit(DISTRIBUTE_ADDRESS_GATE_MAP):
        this->sendAddressGateMap();
        this->receiving_count = 0;
        delete msg;
        FSM_Goto(fsm, WAITING_FOR_RESPONSE_GATE_MAP);
        break;

    case FSM_Enter(WAITING_FOR_RESPONSE_GATE_MAP):
        break;

    case FSM_Exit(WAITING_FOR_RESPONSE_GATE_MAP):
        this->receiving_count += 1;
        delete msg;
        if (this->receiving_count == this->nodeid2mod.size())
        {
            FSM_Goto(fsm, DISTRIBUTE_ROUTING);
        }
        break;
    case FSM_Exit(DISTRIBUTE_ROUTING):
        this->sendRoutingTable();
        this->receiving_count = 0;
        FSM_Goto(fsm, WAITING_FOR_RESPONSE_ROUTING);
        break;
    case FSM_Enter(WAITING_FOR_RESPONSE_ROUTING):
        break;
    case FSM_Exit(WAITING_FOR_RESPONSE_ROUTING):
        this->receiving_count += 1;
        delete msg;
        if (this->receiving_count == this->nodeid2mod.size())
        {
            this->receiving_count = 0;
            FSM_Goto(fsm, DISTRIBUTE_TRAFFIC_MATRIX);
        }
    case FSM_Exit(DISTRIBUTE_TRAFFIC_MATRIX):
        this->trafficMatrix.generateRandomTrafficMatrix(this->nodeid2mod.size(), this->trafficFactor);
        EV << "generate traffix matrix with size " << this->nodeid2mod.size() << " " << this->nodeid2mod.size() << std::endl;
        this->trafficMatrix.serializeTrafficMatrix("./results/traffix_matrix.txt");
        this->sendTrafficMatrix();
        FSM_Goto(fsm, WAITING_FOR_RESPONSE_TRAFFIC);
        break;
    case FSM_Enter(WAITING_FOR_RESPONSE_TRAFFIC):
        break;
    case FSM_Exit(WAITING_FOR_RESPONSE_TRAFFIC):
        this->receiving_count += 1;
        delete msg;
        if (this->receiving_count == this->nodeid2mod.size())
        {
            FSM_Goto(fsm, FINISH);
        }
        break;
    case FSM_Enter(FINISH):
        break;
    case FSM_Exit(FINISH):
        delete msg;
        break;
    };
}

void NetBuilder::connect(cGate *src, cGate *dest, double delay, double ber, double datarate)
{
    cDatarateChannel *channel = nullptr;
    if (delay > 0 || ber > 0 || datarate > 0)
    {
        channel = cDatarateChannel::create("channel");
        if (delay > 0)
            channel->setDelay(delay);
        if (ber > 0)
            channel->setBitErrorRate(ber);
        if (datarate > 0)
            channel->setDatarate(datarate);
    }
    src->connectTo(dest, channel);
}

void NetBuilder::sendAddressGateMap()
{
    int gate = 0;
    for (auto addressGateTable : addressGateTableList)
    {
        std::stringstream ss;
        for (auto addressGateColumn : addressGateTable.second)
        {
            ss << addressGateColumn.first << " " << addressGateColumn.second << std::endl;
        }

        auto addressGateMessage = generateControlPacket("address-gate map", ss.str());
        send(addressGateMessage, "out", gate);
        gate++;
    }
}

void NetBuilder::sendRoutingTable()
{

    this->routingTable.transformPathToTable();
    this->routingTable.serializeRoutingPath("./results/routing_path.txt");
    this->routingTable.serializeRoutingTable("./results/routing_table.txt");

    const auto &routerTableList = this->routingTable.getRoutingTable();
    int gate = 0;
    for (auto routertable : routerTableList)
    {
        std::stringstream ss;
        for (auto routerColumn : routertable.second)
        {
            ss << routerColumn.first.first << " " << routerColumn.first.second << " " << routerColumn.second << std::endl;
        }
        auto routerMessage = generateControlPacket("router table", ss.str());
        send(routerMessage, "out", gate);
        gate++;
    }
}

void NetBuilder::sendTrafficMatrix()
{
    int gate = 0;
    for (auto trafficColumn : this->trafficMatrix.getTrafficMatrix())
    {
        std::stringstream ss;
        for (auto trafficDensity : trafficColumn)
        {
            ss << trafficDensity << " ";
        }
        auto trafficMessage = generateControlPacket("traffic matrix", ss.str());
        send(trafficMessage, "out", gate);
        gate++;
    }
}

void NetBuilder::buildNetwork(cModule *parent)
{

    std::string line;
    std::fstream nodesFile(par("nodesFile").stringValue(), std::ios::in);
    while (getline(nodesFile, line, '\n'))
    {
        if (line.empty() || line[0] == '#')
            continue;

        std::vector<std::string> tokens = cStringTokenizer(line.c_str()).asVector();
        if (tokens.size() != 3)
            throw cRuntimeError("wrong line in module file: 3 items required, line: \"%s\"", line.c_str());

        // get fields from tokens
        long nodeid = atol(tokens[0].c_str());
        const char *name = tokens[1].c_str();
        const char *modtypename = tokens[2].c_str();
        EV << "NODE id=" << nodeid << " name=" << name << " type=" << modtypename << "\n";

        // create module
        cModuleType *modtype = cModuleType::find(modtypename);
        if (!modtype)
            throw cRuntimeError("module type `%s' for node `%s' not found", modtypename, name);
        cModule *mod = modtype->create(name, parent);
        nodeid2mod[nodeid] = mod;

        // read params from the ini file, etc
        mod->finalizeParameters();
    }

    topo = new Graph(nodeid2mod.size());

    for (auto &pair : nodeid2mod)
    {
        addressGateTableList[pair.first] = std::map<int, int>();
    }

    // read and create connections
    std::fstream connectionsFile(par("connectionsFile").stringValue(), std::ios::in);
    while (getline(connectionsFile, line, '\n'))
    {
        if (line.empty() || line[0] == '#')
            continue;
        std::vector<std::string> tokens = cStringTokenizer(line.c_str()).asVector();
        if (tokens.size() != 5)
            throw cRuntimeError("wrong line in parameters file: 5 items required, line: \"%s\"", line.c_str());

        // get fields from tokens
        long srcnodeid = atol(tokens[0].c_str());
        long destnodeid = atol(tokens[1].c_str());
        double delay = tokens[2] != "-" ? atof(tokens[2].c_str()) : -1;
        double error = tokens[3] != "-" ? atof(tokens[3].c_str()) : -1;
        double datarate = tokens[4] != "-" ? atof(tokens[4].c_str()) : -1;

        if (nodeid2mod.find(srcnodeid) == nodeid2mod.end())
            throw cRuntimeError("wrong line in connections file: node with id=%ld not found", srcnodeid);
        if (nodeid2mod.find(destnodeid) == nodeid2mod.end())
            throw cRuntimeError("wrong line in connections file: node with id=%ld not found", destnodeid);

        // add edge in boost graph
        // IMPORTANT: boost mark node from 0
        add_edge(srcnodeid - 1, destnodeid - 1, *topo);

        int srcNodeGateSize = static_cast<int>(addressGateTableList[srcnodeid].size());
        int dstNodeGateSize = static_cast<int>(addressGateTableList[destnodeid].size());
        addressGateTableList[srcnodeid][destnodeid] = srcNodeGateSize;
        addressGateTableList[destnodeid][srcnodeid] = dstNodeGateSize;

        cModule *srcmod = nodeid2mod[srcnodeid];
        cModule *destmod = nodeid2mod[destnodeid];

        cGate *srcIn, *srcOut, *destIn, *destOut;
        srcmod->getOrCreateFirstUnconnectedGatePair("port", false, true, srcIn, srcOut);
        destmod->getOrCreateFirstUnconnectedGatePair("port", false, true, destIn, destOut);

        // connect
        connect(srcOut, destIn, delay, error, datarate);
        connect(destOut, srcIn, delay, error, datarate);
    }

    // connect builder with node.
    // TODO: Maybe separate builder and controller.
    for (auto it = nodeid2mod.begin(); it != nodeid2mod.end(); ++it)
    {
        cGate *controllerIn, *controllerOut;
        cGate *nodeIn, *nodeOut;
        controllerIn = this->getOrCreateFirstUnconnectedGate("in", 0, false, true);
        controllerOut = this->getOrCreateFirstUnconnectedGate("out", 0, false, true);
        nodeIn = it->second->gate("controllerPort$i");
        nodeOut = it->second->gate("controllerPort$o");

        connect(controllerOut, nodeIn, 0.01, 0, 1e6);
        connect(nodeOut, controllerIn, 0.01, 0, 1e6);
    }

    std::map<long, cModule *>::iterator it;

    // final touches: buildinside, initialize()
    for (it = nodeid2mod.begin(); it != nodeid2mod.end(); ++it)
    {
        cModule *mod = it->second;
        mod->buildInside();
    }

    // multi-stage init
    bool more = true;
    for (int stage = 0; more; stage++)
    {
        more = false;
        for (it = nodeid2mod.begin(); it != nodeid2mod.end(); ++it)
        {
            cModule *mod = it->second;
            if (mod->callInitialize(stage))
                more = true;
        }
    }

    typedef property_map<Graph, vertex_index_t>::type IndexMap;
    IndexMap topo_index = get(vertex_index, *topo);

#ifdef DEBUG
    for (auto vp = vertices(*topo); vp.first != vp.second; ++vp.first)
    {
        auto v = *vp.first;
        EV << "adjacent vertices of " << topo_index[v] << " is: ";
        typename graph_traits<Graph>::adjacency_iterator ai;
        typename graph_traits<Graph>::adjacency_iterator ai_end;
        for (boost::tie(ai, ai_end) = adjacent_vertices(v, *topo);
             ai != ai_end; ++ai)
            EV << topo_index[*ai] << " ";
        EV << std::endl;
    }
#endif

    for (auto vp = vertices(*topo); vp.first != vp.second; ++vp.first)
    {
        // destination node
        auto dst = *vp.first;

        std::vector<graph_traits<Graph>::vertex_descriptor> p(num_vertices(*topo));
        auto predmap = boost::make_iterator_property_map(p.begin(), topo_index);

        random_spanning_tree(*topo, rng, root_vertex(dst).predecessor_map(predmap));

        for (auto vp_inner = vertices(*topo); vp_inner.first != vp_inner.second; ++vp_inner.first)
        {
            auto src = *vp_inner.first;
            auto src_dst_pair = std::make_pair(topo_index[src] + 1, topo_index[dst] + 1);

            if (src == dst)
            { // no routing table needed.
                continue;
            }

            std::list<int> path;
            path.emplace_back(src + 1);

            if (predmap[topo_index[src]] == topo_index[dst])
            {
                path.emplace_back(topo_index[dst] + 1);
            }
            else
            {
                auto predecessor = topo_index[src];
                while (predecessor != topo_index[dst])
                {
                    path.emplace_back(predmap[predecessor] + 1);
                    predecessor = predmap[predecessor];
                }
            }
            routingTable.addRoutingforPath(std::move(src_dst_pair), std::move(path));
        }
    }
}


#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>

#include <boost/dynamic_bitset.hpp>
#include <cassert>
#include <cstring>
#include <fstream>
#include <iostream>
#include <string>
#include <utility>
#include <vector>

#include "type.hpp"
#include "utility.hpp"
#ifdef _USE_GPU
#include <gpusim/stat_ops.h>
#endif

#include <csim/stat_ops.hpp>
#include <csim/stat_ops_dm.hpp>

#include "gate_factory.hpp"
#include "pauli_operator.hpp"
#include "state.hpp"

PauliOperator::PauliOperator(std::string strings, CPPCTYPE coef) : _coef(coef) {
    std::stringstream ss(strings);
    std::string pauli_str;
    UINT index, pauli_type = 0;
    while (!ss.eof()) {
        ss >> pauli_str >> index;
        if (pauli_str.length() == 0) break;
        if (pauli_str == "I" || pauli_str == "i")
            pauli_type = 0;
        else if (pauli_str == "X" || pauli_str == "x")
            pauli_type = 1;
        else if (pauli_str == "Y" || pauli_str == "y")
            pauli_type = 2;
        else if (pauli_str == "Z" || pauli_str == "z")
            pauli_type = 3;
        else {
            fprintf(stderr, "invalid Pauli string is given : %s\n ",
                pauli_str.c_str());
            assert(false);
        }
        if (pauli_type != 0) this->add_single_Pauli(index, pauli_type);
    }
}

PauliOperator::PauliOperator(const std::vector<UINT>& target_qubit_list,
    std::string Pauli_operator_type_list, CPPCTYPE coef)
    : _coef(coef) {
    UINT term_count = (UINT)(strlen(Pauli_operator_type_list.c_str()));
    UINT pauli_type = 0;
    for (UINT term_index = 0; term_index < term_count; ++term_index) {
        if (Pauli_operator_type_list[term_index] == 'i' ||
            Pauli_operator_type_list[term_index] == 'I') {
            pauli_type = 0;
        } else if (Pauli_operator_type_list[term_index] == 'x' ||
                   Pauli_operator_type_list[term_index] == 'X') {
            pauli_type = 1;
        } else if (Pauli_operator_type_list[term_index] == 'y' ||
                   Pauli_operator_type_list[term_index] == 'Y') {
            pauli_type = 2;
        } else if (Pauli_operator_type_list[term_index] == 'z' ||
                   Pauli_operator_type_list[term_index] == 'Z') {
            pauli_type = 3;
        } else {
            fprintf(stderr, "invalid Pauli string is given\n");
            assert(false);
        }

        if (pauli_type != 0)
            this->add_single_Pauli(target_qubit_list[term_index], pauli_type);
    }
}

PauliOperator::PauliOperator(const std::vector<UINT>& pauli_list, CPPCTYPE coef)
    : _coef(coef) {
    for (UINT term_index = 0; term_index < pauli_list.size(); ++term_index) {
        if (pauli_list[term_index] != 0)
            this->add_single_Pauli(term_index, pauli_list[term_index]);
    }
}

PauliOperator::PauliOperator(const std::vector<UINT>& target_qubit_index_list,
    const std::vector<UINT>& target_qubit_pauli_list, CPPCTYPE coef)
    : _coef(coef) {
    assert(target_qubit_index_list.size() == target_qubit_pauli_list.size());
    for (UINT term_index = 0; term_index < target_qubit_index_list.size();
         ++term_index) {
        this->add_single_Pauli(target_qubit_index_list[term_index],
            target_qubit_pauli_list[term_index]);
    }
}

PauliOperator::PauliOperator(const boost::dynamic_bitset<>& x,
    const boost::dynamic_bitset<>& z, CPPCTYPE coef) {
    _coef = coef;
    for (UINT i = 0; i < x.size(); i++) {
        UINT pauli_type = 0;
        if (x[i] && !z[i]) {
            pauli_type = 1;
        } else if (x[i] && z[i]) {
            pauli_type = 2;
        } else if (!x[i] && z[i]) {
            pauli_type = 3;
        }
        if (pauli_type != 0) {
            this->add_single_Pauli(i, pauli_type);
        }
    }
}

void PauliOperator::add_single_Pauli(UINT qubit_index, UINT pauli_type) {
    this->_pauli_list.push_back(SinglePauliOperator(qubit_index, pauli_type));
    while (_x.size() <= qubit_index) {
        _x.resize(_x.size() * 2 + 1);
        _z.resize(_z.size() * 2 + 1);
    }
    if (pauli_type == 1) {
        _x.set(qubit_index);
    } else if (pauli_type == 2) {
        _x.set(qubit_index);
        _z.set(qubit_index);
    } else if (pauli_type == 3) {
        _z.set(qubit_index);
    }
}

CPPCTYPE PauliOperator::get_expectation_value(
    const QuantumStateBase* state) const {
    if (state->is_state_vector()) {
#ifdef _USE_GPU
        if (state->get_device_name() == "gpu") {
            return _coef *
                   expectation_value_multi_qubit_Pauli_operator_partial_list_host(
                       this->get_index_list().data(),
                       this->get_pauli_id_list().data(),
                       (UINT)this->get_index_list().size(), state->data(),
                       state->dim, state->get_cuda_stream(),
                       state->device_number);
        } else {
            return _coef *
                   expectation_value_multi_qubit_Pauli_operator_partial_list(
                       this->get_index_list().data(),
                       this->get_pauli_id_list().data(),
                       (UINT)this->get_index_list().size(), state->data_c(),
                       state->dim);
        }
#else
        return _coef *
               expectation_value_multi_qubit_Pauli_operator_partial_list(
                   this->get_index_list().data(),
                   this->get_pauli_id_list().data(),
                   (UINT)this->get_index_list().size(), state->data_c(),
                   state->dim);
#endif
    } else {
        return _coef *
               dm_expectation_value_multi_qubit_Pauli_operator_partial_list(
                   this->get_index_list().data(),
                   this->get_pauli_id_list().data(),
                   (UINT)this->get_index_list().size(), state->data_c(),
                   state->dim);
    }
}

CPPCTYPE PauliOperator::get_transition_amplitude(
    const QuantumStateBase* state_bra,
    const QuantumStateBase* state_ket) const {
    if ((!state_bra->is_state_vector()) || (!state_ket->is_state_vector())) {
        std::cerr
            << "get_transition_amplitude for density matrix is not implemented"
            << std::endl;
    }
#ifdef _USE_GPU
    if (state_ket->get_device_name() == "gpu" &&
        state_bra->get_device_name() == "gpu") {
        return _coef *
               (CPPCTYPE)
                   transition_amplitude_multi_qubit_Pauli_operator_partial_list_host(
                       this->get_index_list().data(),
                       this->get_pauli_id_list().data(),
                       (UINT)this->get_index_list().size(), state_bra->data(),
                       state_ket->data(), state_bra->dim,
                       state_ket->get_cuda_stream(), state_ket->device_number);
    } else {
        return _coef *
               (CPPCTYPE)
                   transition_amplitude_multi_qubit_Pauli_operator_partial_list(
                       this->get_index_list().data(),
                       this->get_pauli_id_list().data(),
                       (UINT)this->get_index_list().size(), state_bra->data_c(),
                       state_ket->data_c(), state_bra->dim);
    }
#else
    return _coef *
           (CPPCTYPE)
               transition_amplitude_multi_qubit_Pauli_operator_partial_list(
                   this->get_index_list().data(),
                   this->get_pauli_id_list().data(),
                   (UINT)this->get_index_list().size(), state_bra->data_c(),
                   state_ket->data_c(), state_bra->dim);
#endif
}

PauliOperator* PauliOperator::copy() const {
    auto pauli = new PauliOperator(this->_coef);
    for (auto val : this->_pauli_list) {
        pauli->add_single_Pauli(val.index(), val.pauli_id());
    }
    return pauli;
}

std::string PauliOperator::get_pauli_string() const {
    std::string res = "";
    UINT size = _pauli_list.size();
    UINT target_index, pauli_id;
    if (size == 0) {
        return "I";
    }
    for (UINT index = 0; index < size; index++) {
        target_index = _pauli_list[index].index();
        pauli_id = _pauli_list[index].pauli_id();
        if (pauli_id == 0)
            continue;
        else if (pauli_id == 1)
            res += "X";
        else if (pauli_id == 2)
            res += "Y";
        else if (pauli_id == 3)
            res += "Z";
        res += " " + std::to_string(target_index) + " ";
    }
    res.pop_back();
    return res;
}

void PauliOperator::change_coef(CPPCTYPE new_coef) { _coef = new_coef; }

PauliOperator PauliOperator::operator*(const PauliOperator& target) const {
    CPPCTYPE bits_coef = 1.0;
    CPPCTYPE I = 1.0i;
    auto x = _x;
    auto z = _z;
    auto target_x = target.get_x_bits();
    auto target_z = target.get_x_bits();
    if (target_x.size() != _x.size()) {
        ITYPE max_size = std::max(_x.size(), target_x.size());
        x.resize(max_size);
        z.resize(max_size);
        target_x.resize(max_size);
        target_z.resize(max_size);
    }
    ITYPE i;
#pragma omp parallel for
    for (i = 0; i < x.size(); i++) {
        if (x[i] && !z[i]) {  // X
            if (!target_x[i] && target_z[i]) {
                bits_coef = bits_coef * -I;
            } else if (target_x[i] && target_z[i]) {
                bits_coef = bits_coef * I;
            }
        } else if (!x[i] && z[i]) {             // Z
            if (target_x[i] && !target_z[i]) {  // X
                bits_coef = bits_coef * -I;
            } else if (target_x[i] && target_z[i]) {  // Y
                bits_coef = bits_coef * I;
            }
        } else if (x[i] && z[i]) {              // Y
            if (target_x[i] && !target_z[i]) {  // X
                bits_coef = bits_coef * I;
            } else if (!target_x[i] && target_z[i]) {  // Z
                bits_coef = bits_coef * I;
            }
        }
    }
    PauliOperator res(
        x ^ target_x, z ^ target_z, _coef * target.get_coef() * bits_coef);
    return res;
}

PauliOperator PauliOperator::operator*(CPPCTYPE target) const {
    PauliOperator res(_x, _z, _coef * target);
    return res;
}

PauliOperator& PauliOperator::operator*=(const PauliOperator& target) {
    _coef *= target.get_coef();
    CPPCTYPE I = 1.0i;
    auto target_x = target.get_x_bits();
    auto target_z = target.get_z_bits();
    ITYPE max_size = std::max(_x.size(), target_x.size());
    if (target_x.size() != _x.size()) {
        _x.resize(max_size);
        _z.resize(max_size);
        target_x.resize(max_size);
        target_z.resize(max_size);
    }
    ITYPE i;
#pragma omp parallel for
    for (i = 0; i < _x.size(); i++) {
        if (_x[i] && !_z[i]) {  // X
            if (!target_x[i] && target_z[i]) {
                _coef *= -I;
            } else if (target_x[i] && target_z[i]) {
                _coef *= I;
            }
        } else if (!_x[i] && _z[i]) {           // Z
            if (target_x[i] && !target_z[i]) {  // X
                _coef *= -I;
            } else if (target_x[i] && target_z[i]) {  // Y
                _coef *= I;
            }
        } else if (_x[i] && _z[i]) {            // Y
            if (target_x[i] && !target_z[i]) {  // X
                _coef *= I;
            } else if (!target_x[i] && target_z[i]) {  // Z
                _coef *= I;
            }
        }
    }
    auto x_bit = _x ^ target_x;
    auto z_bit = _z ^ target_z;
    _x.clear();
    _z.clear();
    _pauli_list.clear();
    _x.resize(max_size);
    _z.resize(max_size);
#pragma omp parallel for
    for (i = 0; i < x_bit.size(); i++) {
        ITYPE pauli_type = 0;
        if (x_bit[i] && !z_bit[i]) {
            pauli_type = 1;
        } else if (x_bit[i] && z_bit[i]) {
            pauli_type = 2;
        } else if (!x_bit[i] && z_bit[i]) {
            pauli_type = 3;
        }
        if (pauli_type != 0) {
            this->add_single_Pauli(i, pauli_type);
        }
    }
    return *this;
}

PauliOperator& PauliOperator::operator*=(CPPCTYPE target) {
    _coef *= target;
    return *this;
}

// made by watle
void PauliOperator::update_quantum_state(QuantumStateBase* instate) {
    // PauliOperator　wo gate tosite kanngaeru
    std::vector<UINT> index_list = this->get_index_list();
    std::vector<UINT> pauli_list = this->get_pauli_id_list();
    for (size_t ii = 0; ii < index_list.size(); ii++) {
        if (pauli_list[ii] == 1) {
            auto x_gate = gate::X(index_list[ii]);
            x_gate->update_quantum_state(instate);
            delete x_gate;
        } else if (pauli_list[ii] == 2) {
            auto y_gate = gate::Y(index_list[ii]);
            y_gate->update_quantum_state(instate);
            delete y_gate;
        } else if (pauli_list[ii] == 3) {
            auto z_gate = gate::Z(index_list[ii]);
            z_gate->update_quantum_state(instate);
            delete z_gate;
        }
    }
    instate->multiply_coef(this->get_coef());
    return;
}


#ifndef ASLAM_OMNI_UNDISTORTER_HPP
#define ASLAM_OMNI_UNDISTORTER_HPP

#include <aslam/Frame.hpp>
#include <aslam/cameras.hpp>
#include <aslam/cameras/CameraGeometry.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/assert_macros.hpp>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <aslam/UndistorterBase.hpp>

namespace cv {
class Mat;
}  // namespace cv

namespace aslam {

/// \brief A class for undistorting Images with the Omni Camera Model.
template <typename MASK_T>
class OmniUndistorter : public UndistorterBase {
  public:
    typedef cameras::CameraGeometry<cameras::OmniProjection<cameras::RadialTangentialDistortion>,
                                    cameras::GlobalShutter, MASK_T>
        distorted_geometry_t;
    typedef cameras::CameraGeometry<cameras::OmniProjection<cameras::NoDistortion>, cameras::GlobalShutter, MASK_T>
        ideal_geometry_t;
    typedef cameras::CameraGeometry<cameras::PinholeProjection<cameras::NoDistortion>, cameras::GlobalShutter, MASK_T>
        ideal_pinhole_geometry_t;
    typedef Frame<ideal_geometry_t> frame_t;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    OmniUndistorter();
    OmniUndistorter(const sm::PropertyTree& undistorterConfig, const sm::PropertyTree& cameraConfig);
    /// \param[in] cameraConfig         A single camera part of a Property Tree is expected.
    /// \param[in] alpha                The free scaling parameter between 0 (when all the pixels in the undistorted
    /// image will be valid) and 1 (when all the source image pixels will be retained in the undistorted image).
    /// \param[in] scale                Allows to scale the resulting image.
    OmniUndistorter(const sm::PropertyTree& cameraConfig, int interpolation, double alpha, double scale);
    /// \param[in] alpha                The free scaling parameter between 0 (when all the pixels in the undistorted
    /// image will be valid) and 1 (when all the source image pixels will be retained in the undistorted image).
    /// \param[in] scale                Allows to scale the resulting image.
    OmniUndistorter(boost::shared_ptr<distorted_geometry_t> distortedGeometry, int interpolation, double alpha,
                    double scale);
    /// \param[in] distortedGeometry    the distorted geometry type
    /// \param[in] config               a configuration object for the other parameters
    OmniUndistorter(boost::shared_ptr<distorted_geometry_t> distortedGeometry, const sm::PropertyTree& config);
    ~OmniUndistorter();

    /// \brief Initialize the Undistorter.
    /// \param[in] alpha                The free scaling parameter between 0 (when all the pixels in the undistorted
    /// image will be valid) and 1 (when all the source image pixels will be retained in the undistorted image).
    /// \param[in] scale                Allows to scale the resulting image.
    void init(boost::shared_ptr<distorted_geometry_t> distortedGeometry, int interpolation, double alpha, double scale);

    /// \brief this undistorts the image and sets both the image and the geometry in the out frame.
    void constructUndistortedFrame(const cv::Mat& inImage, frame_t& outFrame) const;

    /// \brief Undistort a single image.
    void undistortImage(const cv::Mat& inImage, cv::Mat& outImage) const;
    void undistortImageToPinhole(const cv::Mat& inImage, cv::Mat& outImage) const;

    boost::shared_ptr<ideal_pinhole_geometry_t> idealPinholeGeometry() const;
    boost::shared_ptr<cameras::CameraGeometryBase> idealPinholeGeometryBase() const { return _idealPinholeGeometry(); }

    boost::shared_ptr<ideal_geometry_t> idealGeometry() const;
    boost::shared_ptr<cameras::CameraGeometryBase> idealGeometryBase() const { return idealGeometry(); }

    boost::shared_ptr<distorted_geometry_t> distortedGeometry() const;
    boost::shared_ptr<cameras::CameraGeometryBase> distortedGeometryBase() const { return distortedGeometry(); }

    virtual boost::shared_ptr<FrameBase> buildFrame(cv::Mat& inImage);

  private:
    /// \brief Set ideal geometry and create undistortion maps.
    void setIdealGeometry(boost::shared_ptr<distorted_geometry_t> distortedGeometry, double alpha, double scale);

    /// \brief Set ideal pinhole geometry and create omni to pinhole maps.
    void setIdealPinholeGeometry(boost::shared_ptr<ideal_geometry_t> idealGeometry);

    boost::shared_ptr<ideal_geometry_t> _idealGeometry;
    boost::shared_ptr<ideal_pinhole_geometry_t> _idealPinholeGeometry;
    boost::shared_ptr<distorted_geometry_t> _distortedGeometry;

    // mapping distorted omni --> undistorted omni
    cv::Mat mapX, mapY;

    // mapping undistorted omni --> undistorted pinhole
    cv::Mat mapX_pinhole, mapY_pinhole;

    int _interpolation;
};

}  // namespace aslam

#include "implementation/OmniUndistorter.hpp"

#endif /* ASLAM_OMNI_UNDISTORTER_HPP */


#define BOOST_TEST_MODULE parser
#include <boost/test/included/unit_test.hpp>

#include "../src/parse/number.h"

BOOST_AUTO_TEST_SUITE(ouinet_parser)

using namespace std;
using namespace ouinet;
using string_view = boost::string_view;

BOOST_AUTO_TEST_CASE(test_unsigned_number) {
    {
        string_view s = "0";
        auto on = parse::number<unsigned>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(0u, *on);
    }

    {
        string_view s = "1234";
        auto on = parse::number<unsigned>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234u, *on);
    }

    {
        string_view s = "01234";
        auto on = parse::number<unsigned>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234u, *on);
    }

    {
        string_view s = "+01234";
        string_view s_ = s;
        BOOST_REQUIRE(!parse::number<unsigned>(s));
        // Don't consume if parse fails
        BOOST_REQUIRE_EQUAL(s.size(), s_.size());
    }

    {
        string_view s = "-01234";
        BOOST_REQUIRE(!parse::number<unsigned>(s));
    }

    {
        // Max uint8_t
        string_view s = "255";
        auto on = parse::number<uint8_t>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(255, *on);
    }

    {
        // Too big
        string_view s = "256";
        BOOST_REQUIRE(!parse::number<uint8_t>(s));
    }
}

BOOST_AUTO_TEST_CASE(test_signed_number) {
    {
        string_view s = "1234";
        auto on = parse::number<int>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234, *on);
    }

    {
        string_view s = "+1234";
        auto on = parse::number<int>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234, *on);
    }

    {
        string_view s = "-1234";
        auto on = parse::number<int>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(-1234, *on);
    }

    {
        string_view s = "01234";
        auto on = parse::number<int>(s);

        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234, *on);
    }

    {
        string_view s = "+01234";
        auto on = parse::number<int>(s);
        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(1234, *on);
    }

    {
        string_view s = "+a";
        auto on = parse::number<int>(s);
        BOOST_REQUIRE(!on);
        // Don't consume if not parsed
        BOOST_REQUIRE_EQUAL(s.size(), 2u);
    }

    {
        string_view s = "-01234";
        auto on = parse::number<int>(s);
        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(-1234, *on);
    }

    {
        // Max char
        string_view s = "127";
        auto on = parse::number<char>(s);
        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(127, *on);
    }

    {
        // Min char
        string_view s = "-128";
        auto on = parse::number<char>(s);
        BOOST_REQUIRE(on);
        BOOST_REQUIRE_EQUAL(-128, *on);
    }

    {
        // Too big
        string_view s = "128";
        BOOST_REQUIRE(!parse::number<char>(s));
    }

    {
        // Too small
        string_view s = "-129";
        BOOST_REQUIRE(!parse::number<char>(s));
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//          Copyright 2015 J.T.  Lapreste
//
//          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_BITWISE_FUNCTIONS_GENERIC_SWAPBYTES_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_GENERIC_SWAPBYTES_HPP_INCLUDED

#include <boost/simd/bitwise/functions/swapbytes.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT( swapbytes_, tag::cpu_
                          , (A0)
                          , (generic_< ints8_<A0> >)
                          )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };
  BOOST_DISPATCH_IMPLEMENT( swapbytes_, tag::cpu_
                          , (A0)
                          , (generic_< ints16_<A0> >)
                          )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
     return shl(a0, 8)|shr(a0, 8);
    }
  };
  BOOST_DISPATCH_IMPLEMENT( swapbytes_, tag::cpu_
                          , (A0)
                          , (generic_< type32_<A0> >)
                          )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type val = ((shl(a0, 8) & 0xFF00FF00 ) | (shr(a0, 8) & 0xFF00FF ));
      return shl(val, 16) | shr(val,16);
    }
  };
  BOOST_DISPATCH_IMPLEMENT( swapbytes_, tag::cpu_
                          , (A0)
                          , (generic_< type64_<A0> >)
                          )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type val = (shl(a0, 8) & 0xFF00FF00FF00FF00ULL ) | (shr(a0, 8) & 0x00FF00FF00FF00FFULL );
      val = (shl(val, 16) & 0xFFFF0000FFFF0000ULL ) | (shr(val, 16) & 0x0000FFFF0000FFFFULL );
      return shl(val, 32) | shr(val,32);
    }
  };
} } }

#endif