#ifndef NN_LAYER_H_
#define NN_LAYER_H_

#include <vector>
#include <cstddef>
#include <stdexcept>
#include <initializer_list>
#include <iostream>
#include <utility>
#include <random>
#include <Eigen/Core>
#include "../common/graphs.hpp"
#include "activator.hpp"
#include "integrator.hpp"
#include "../common/multi_array.hpp"
#include "parameter_types.hpp"
#include "../random/pcg_random.hpp"

namespace nervous_system {

/*
 * Layers take ownership of the integrators and activations functions they use.
 * This is necessary as the Layer will be the only remaining access point to
 * those objects once it is pushed off to the nervous system.
 */
template<typename TReal>
class Layer {
  public:
    typedef std::size_t Index;
    typedef float Factor;

    Layer() {
      back_integrator_ = nullptr;
      self_integrator_ = nullptr;
      activation_function_ = nullptr;
      parameter_count_ = 0;
    }

    Layer(const std::vector<Index>& shape,
          Integrator<TReal>* back_integrator,
          Integrator<TReal>* self_integrator,
          Activator<TReal>* activation_function)
          : back_integrator_(back_integrator),
            self_integrator_(self_integrator),
            activation_function_(activation_function),
            layer_state_(shape), input_buffer_(shape), shape_(shape) {
      parameter_count_ = 0;
      if (back_integrator_ != nullptr)
      {
        parameter_count_ += back_integrator_->GetParameterCount();
      }
      if (self_integrator_ != nullptr)
      {
        parameter_count_ += self_integrator_->GetParameterCount();
      }
      if (activation_function_ != nullptr)
      {
        parameter_count_ += activation_function_->GetParameterCount();
      }
    }

    virtual ~Layer() {
      delete back_integrator_;
      delete self_integrator_;
      delete activation_function_;
    }

    /*
     * Update neuron state. Calls both integrator and activator.
     */
    virtual void operator()(const Layer<TReal>* prev_layer) {
      // First clear input buffer
      input_buffer_.Fill(0.0);

      // Call back integrator first to resolve input from prev layer
      (*back_integrator_)(prev_layer->state(), input_buffer_);

      // Resolve self-connections if there are any
      (*self_integrator_)(input_buffer_, input_buffer_);

      // Apply activation and update state
      (*activation_function_)(layer_state_, input_buffer_);
    }

    /*
     * Passes the needed parameters to the Layer - Should be Slice with
     * parameter_count_ in size. Layer will then make and assign Slices to
     * The activation_function and Integrator function
     */
    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {

      if (parameters.size() != parameter_count_) {
        std::cerr << "parameter size: " << parameters.size() << std::endl;
        std::cerr << "parameter count: " << parameter_count_ << std::endl;
        throw std::invalid_argument("Wrong number of parameters given.");
      }

      back_integrator_->Configure(
        parameters.slice(0, back_integrator_->GetParameterCount()));

      self_integrator_->Configure(
        parameters.slice(parameters.stride() * back_integrator_->GetParameterCount(),
                         self_integrator_->GetParameterCount()));

      activation_function_->Configure(
        parameters.slice(parameters.stride() * back_integrator_->GetParameterCount()
                         + parameters.stride() * self_integrator_->GetParameterCount(),
                         activation_function_->GetParameterCount()));
    }

    virtual void Reset() {
      layer_state_.Fill(0.0);
      input_buffer_.Fill(0.0);
      activation_function_->Reset();
    }

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

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {

      std::vector<PARAMETER_TYPE> layout(parameter_count_);

      // layout produced in configure order: back->self->act
      Index order = 0;
      std::vector<PARAMETER_TYPE> back_layout = back_integrator_->GetParameterLayout();
      for (auto par_type_ptr = back_layout.begin();
          par_type_ptr != back_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }
      std::vector<PARAMETER_TYPE> self_layout = self_integrator_->GetParameterLayout();
      for (auto par_type_ptr = self_layout.begin();
          par_type_ptr != self_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }
      std::vector<PARAMETER_TYPE> act_layout = activation_function_->GetParameterLayout();
      for (auto par_type_ptr = act_layout.begin();
          par_type_ptr != act_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      return layout;
    }

    /*
     * Constructs a normalization factor vector. Each element corresponds to
     * a parameter. Parameters associated with weights should be set to a
     * non-zero value == to the degree of the post-synaptic neuron.
     */
    virtual std::vector<Factor> GetWeightNormalizationFactors() const {
      std::vector<Factor> normalization_factors(parameter_count_);
      // initializes values to 0
      for (auto& factor : normalization_factors) {
        factor = 0.0;
      }

      EvaluateNormalizationFactors(normalization_factors, back_integrator_,
                                   self_integrator_, NumNeurons());

      return normalization_factors;
    }

    std::size_t NumNeurons() const {
      return layer_state_.size();
    }

    template<typename T>
    void SetNeuronState(Index neuron, T value) {
      layer_state_[neuron] = value;
    }

    const multi_array::Tensor<TReal>& state() const {
      return layer_state_;
    }

    multi_array::Tensor<TReal>& state() {
      return layer_state_;
    }

    const std::vector<Index>& shape() const {
      return shape_;
    }

    const Integrator<TReal>* GetBackIntegrator() const {
      return back_integrator_;
    }

    const Integrator<TReal>* GetSelfIntegrator() const {
      return self_integrator_;
    }

  protected:
    // calculates inputs from other layers and applies them to input buffer
    Integrator<TReal>* back_integrator_;
    // claculates inputs from neurons within the layer and applies them to input buffer
    Integrator<TReal>* self_integrator_;
    // updates the layer's state using the input buffer (may also contain internal state)
    Activator<TReal>* activation_function_;
    // maintains the current layer's state
    multi_array::Tensor<TReal> layer_state_;
    // holds input values used to update the layer's state
    multi_array::Tensor<TReal> input_buffer_;
    std::vector<Index> shape_;
    // Number of parameters required by layer
    std::size_t parameter_count_;
};

template <typename TReal>
class RecurrentLayer : public Layer<TReal>
{
public:
  typedef Layer<TReal> super_type;
  typedef typename super_type::Index Index;
  typedef Eigen::Matrix<TReal, Eigen::Dynamic, 1> ColVector;
  typedef const Eigen::Matrix<TReal, Eigen::Dynamic, 1> ConstColVector;
  typedef Eigen::Map<ColVector> ColVectorView;
  typedef const Eigen::Map<ConstColVector> ConstColVectorView;

  RecurrentLayer(const std::vector<Index>& shape,
        Integrator<TReal>* back_integrator,
        Integrator<TReal>* self_integrator,
        Activator<TReal>* activation_function)
   : super_type(shape, back_integrator, self_integrator, activation_function),
     recurrent_state_buffer_(shape)
   {}

  virtual ~RecurrentLayer()=default;

  virtual void Reset()
  {
    super_type::Reset();
    recurrent_state_buffer_.Fill(0.0);
  }

  virtual void operator()(const Layer<TReal>* prev_layer) override
  {
    super_type::input_buffer_.Fill(0.0);
    (*super_type::back_integrator_)(prev_layer->state(),
                                    super_type::input_buffer_);
    (*super_type::self_integrator_)(super_type::layer_state_,
                                    recurrent_state_buffer_);
    ColVectorView state_vector(recurrent_state_buffer_.data(),
                               recurrent_state_buffer_.size());
    ConstColVectorView buffer(super_type::input_buffer_.data(),
                              super_type::input_buffer_.size());
    state_vector += buffer;
    (*super_type::activation_function_)(super_type::input_buffer_,
                                        recurrent_state_buffer_);
    std::swap(super_type::layer_state_, super_type::input_buffer_);
  }

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

template <typename TReal>
class FeedbackLayer : public RecurrentLayer<TReal>
{
public:
  typedef RecurrentLayer<TReal> super_type;
  typedef typename super_type::Index Index;
  typedef Eigen::Matrix<TReal, Eigen::Dynamic, 1> ColVector;
  typedef const Eigen::Matrix<TReal, Eigen::Dynamic, 1> ConstColVector;
  typedef Eigen::Map<ColVector> ColVectorView;
  typedef const Eigen::Map<ConstColVector> ConstColVectorView;

  FeedbackLayer(const std::vector<Index>& shape,
                Integrator<TReal>* back_integrator,
                Integrator<TReal>* self_integrator,
                Activator<TReal>* activation_function,
                Index motor_size,
                Integrator<TReal>* feedback_integrator)
   : super_type(shape, back_integrator, self_integrator, activation_function),
     feedback_state_({motor_size + 1}), // 1 added for reward
     feedback_integrator_(feedback_integrator)
   {
     super_type::parameter_count_ += feedback_integrator_->GetParameterCount();
   }

  virtual ~FeedbackLayer()
  {
    delete feedback_integrator_;
  }

  virtual void Reset() {
    super_type::Reset();
    feedback_state_.Fill(0.0);
  }

  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 given.");
    }

    super_type::back_integrator_->Configure(
      parameters.slice(0, super_type::back_integrator_->GetParameterCount()));

    super_type::self_integrator_->Configure(
      parameters.slice(parameters.stride() * super_type::back_integrator_->GetParameterCount(),
                       super_type::self_integrator_->GetParameterCount()));

    feedback_integrator_->Configure(
      parameters.slice(parameters.stride() * super_type::back_integrator_->GetParameterCount()
                       + parameters.stride() * super_type::self_integrator_->GetParameterCount(),
                       feedback_integrator_->GetParameterCount()));

    super_type::activation_function_->Configure(
      parameters.slice(parameters.stride() * super_type::back_integrator_->GetParameterCount()
                       + parameters.stride() * super_type::self_integrator_->GetParameterCount()
                       + parameters.stride() * feedback_integrator_->GetParameterCount(),
                       super_type::activation_function_->GetParameterCount()));
  }

  virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const {

    std::vector<PARAMETER_TYPE> layout(super_type::parameter_count_);

    // layout produced in configure order: back->self->act
    Index order = 0;
    std::vector<PARAMETER_TYPE> back_layout = super_type::back_integrator_->GetParameterLayout();
    for (auto par_type_ptr = back_layout.begin();
        par_type_ptr != back_layout.end(); ++par_type_ptr) {
      layout[order] = *par_type_ptr;
      ++order;
    }
    std::vector<PARAMETER_TYPE> self_layout = super_type::self_integrator_->GetParameterLayout();
    for (auto par_type_ptr = self_layout.begin();
        par_type_ptr != self_layout.end(); ++par_type_ptr) {
      layout[order] = *par_type_ptr;
      ++order;
    }
    std::vector<PARAMETER_TYPE> feedback_layout = feedback_integrator_->GetParameterLayout();
    for (auto par_type_ptr = feedback_layout.begin();
        par_type_ptr != feedback_layout.end(); ++par_type_ptr) {
      layout[order] = *par_type_ptr;
      ++order;
    }
    std::vector<PARAMETER_TYPE> act_layout = super_type::activation_function_->GetParameterLayout();
    for (auto par_type_ptr = act_layout.begin();
        par_type_ptr != act_layout.end(); ++par_type_ptr) {
      layout[order] = *par_type_ptr;
      ++order;
    }

    return layout;
  }

  virtual void operator()(const Layer<TReal>* prev_layer) override
  {
    super_type::input_buffer_.Fill(0.0);
    (*super_type::back_integrator_)(prev_layer->state(),
                                    super_type::input_buffer_);
    (*super_type::self_integrator_)(super_type::layer_state_,
                                    super_type::recurrent_state_buffer_);
    (*feedback_integrator_)(feedback_state_,
                            super_type::layer_state_);
    ColVectorView state_vector(super_type::layer_state_.data(),
                               super_type::layer_state_.size());
    ColVectorView recurrent_state(super_type::recurrent_state_buffer_.data(),
                                  super_type::recurrent_state_buffer_.size());
    ConstColVectorView buffer(super_type::input_buffer_.data(),
                              super_type::input_buffer_.size());
    state_vector += buffer;
    state_vector += recurrent_state;
    (*super_type::activation_function_)(super_type::input_buffer_,
                                        super_type::recurrent_state_buffer_);
    std::swap(super_type::layer_state_, super_type::input_buffer_);
  }

  virtual void update_feedback(TReal reward, const Layer<TReal>* motor_layer)
  {
    auto motor_state = motor_layer->state();
    Index state_size(motor_state.size());
    for (Index i = 0; i < (state_size - 1); ++i)
    {
      feedback_state_[i] = motor_state[i];
    }
    // Temporary hack to set the reward [0,1] at same scale as inputs (255)
    if (reward > 0.00001)
    {
      feedback_state_[feedback_state_.size() - 1] = 255.0;
    }
    else
    {
      feedback_state_[feedback_state_.size() - 1] = 0.0;
    }
  }

  protected:
    multi_array::Tensor<TReal> feedback_state_;
    Integrator<TReal>* feedback_integrator_;
};

template <typename TReal>
class RewardModulatedLayer : public Layer<TReal> {
  public:
    typedef Layer<TReal> super_type;
    typedef typename super_type::Index Index;

    RewardModulatedLayer(const std::vector<Index>& shape,
                         Integrator<TReal>* back_integrator,
                         Integrator<TReal>* self_integrator,
                         Activator<TReal>* activation_function,
                         TReal reward_smoothing_factor,
                         TReal activation_smoothing_factor)
        : super_type(shape, back_integrator, self_integrator,
                     activation_function),
          activation_averages_({super_type::input_buffer_.size()}),
          reward_average_(0.0),
          reward_smoothing_factor_(reward_smoothing_factor),
          activation_smoothing_factor_(activation_smoothing_factor)
    {
//      super_type::parameter_count_ += 2; // reward and activation smoothing factors
      Reset();
    }

    virtual ~RewardModulatedLayer()=default;

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) override {
      super_type::Configure(parameters);
//      reward_smoothing_factor_ = utilities::Wrap0to1(parameters[parameters.size()-2]);
//      activation_smoothing_factor_ = utilities::Wrap0to1(parameters[parameters.size()-1]);
    }

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const override {

      std::vector<PARAMETER_TYPE> layout = super_type::GetParameterLayout();
//      layout.push_back(SMOOTHING);
//      layout.push_back(SMOOTHING);
      return layout;
    }

    virtual void Reset() override {
      super_type::Reset();
      for (auto& avg : activation_averages_) {
        avg = 0;
      }
      reward_average_ = 0.0;
    }

    /*
     * Called after all integrators and activators have been called.
     */
    virtual void UpdateWeights(const TReal reward, const Layer<TReal>* prev_layer) {
      // call weight update function using the input_buffer of this layer
      // the input buffer contains the states prior to application of the
      // activation function.
      if (super_type::back_integrator_->GetIntegratorType() == REWARD_MODULATED)
      {
        dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::back_integrator_)->UpdateWeights(
          reward, reward_average_, prev_layer->state(), super_type::input_buffer_,
          activation_averages_);
      }

      if (super_type::self_integrator_->GetIntegratorType() == REWARD_MODULATED)
      {
        dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::self_integrator_)->UpdateWeights(
          reward, reward_average_, prev_layer->state(), super_type::input_buffer_,
          activation_averages_);
      }

      // update rolling averages
      reward_average_ = utilities::ExponentialRollingAverage(reward, reward_average_,
                                                             reward_smoothing_factor_);
      for (Index i = 0; i < activation_averages_.size(); ++i) {
        activation_averages_[i] = utilities::ExponentialRollingAverage(super_type::input_buffer_[i],
                                                                       activation_averages_[i],
                                                                       activation_smoothing_factor_);
      }
    }

  protected:
    multi_array::Tensor<TReal> activation_averages_;
    TReal reward_average_;
    TReal reward_smoothing_factor_; // between [0,1]
    TReal activation_smoothing_factor_; // between [0,1]
};

/*
 * This uses the correct equation. We need the noise applied to the input_buffer
 * in order to calculate the correct average.
 */
template <typename TReal>
class NoisyRewardModulatedLayer : public Layer<TReal> {
  public:
    typedef Layer<TReal> super_type;
    typedef typename super_type::Index Index;

    NoisyRewardModulatedLayer(const std::vector<Index>& shape,
                         Integrator<TReal>* back_integrator,
                         Integrator<TReal>* self_integrator,
                         Activator<TReal>* activation_function,
                         TReal reward_smoothing_factor,
                         TReal activation_smoothing_factor,
                         const TReal standard_deviation,
                         const std::uint64_t seed)
    : super_type(shape, back_integrator, self_integrator,
                 activation_function),
      activation_averages_({super_type::input_buffer_.size()}),
      reward_average_(0.0),
      reward_smoothing_factor_(reward_smoothing_factor),
      activation_smoothing_factor_(activation_smoothing_factor),
      standard_deviation_(standard_deviation),
      rng_(seed),
      normal_distribution_{}
    {
      Reset();
    }

    virtual ~NoisyRewardModulatedLayer()=default;

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

    virtual std::vector<PARAMETER_TYPE> GetParameterLayout() const override {

      std::vector<PARAMETER_TYPE> layout = super_type::GetParameterLayout();
      return layout;
    }

    virtual void Reset() override {
      super_type::Reset();
      for (auto& avg : activation_averages_) {
        avg = 0;
      }
      reward_average_ = 0.0;
    }

    virtual void operator()(const Layer<TReal>* prev_layer) {
      // First clear input buffer
      super_type::input_buffer_.Fill(0.0);

      // Call back integrator first to resolve input from prev layer
      (*super_type::back_integrator_)(prev_layer->state(), super_type::input_buffer_);

      // Resolve self-connections if there are any
      (*super_type::self_integrator_)(super_type::input_buffer_,
                                      super_type::input_buffer_);

      // Apply noise and then activation and update state
      ApplyNoise(super_type::input_buffer_);
      (*super_type::activation_function_)(super_type::layer_state_,
                                          super_type::input_buffer_);
    }

    /*
     * Called after all integrators and activators have been called.
     */
    virtual void UpdateWeights(const TReal reward, const Layer<TReal>* prev_layer) {
      // call weight update function using the input_buffer of this layer
      // the input buffer contains the states prior to application of the
      // activation function.
      if (super_type::back_integrator_->GetIntegratorType() == REWARD_MODULATED)
      {
        dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::back_integrator_)->UpdateWeights(
          reward, reward_average_, prev_layer->state(), super_type::input_buffer_,
          activation_averages_);
      }

      if (super_type::self_integrator_->GetIntegratorType() == REWARD_MODULATED)
      {
        dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::self_integrator_)->UpdateWeights(
          reward, reward_average_, prev_layer->state(), super_type::input_buffer_,
          activation_averages_);
      }

      // update rolling averages
      reward_average_ = utilities::ExponentialRollingAverage(reward, reward_average_,
                                                             reward_smoothing_factor_);
      for (Index i = 0; i < activation_averages_.size(); ++i) {
        activation_averages_[i] = utilities::ExponentialRollingAverage(super_type::input_buffer_[i],
                                                                       activation_averages_[i],
                                                                       activation_smoothing_factor_);
      }
    }

    virtual void ApplyNoise(multi_array::Tensor<TReal>& inputs)
    {
      for (Index i = 0; i < inputs.size(); ++i)
      {
        inputs[i] += standard_deviation_ * normal_distribution_(rng_);
      }
    }

  protected:
    multi_array::Tensor<TReal> activation_averages_;
    TReal reward_average_;
    TReal reward_smoothing_factor_; // between [0,1]
    TReal activation_smoothing_factor_; // between [0,1]
    TReal standard_deviation_;
    pcg32_fast rng_;
    std::normal_distribution<TReal> normal_distribution_;
};

template<typename TReal>
class InputLayer : public Layer<TReal> {
  public:
    typedef Layer<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef typename super_type::Factor Factor;

    InputLayer() : super_type() {
    }

    InputLayer(const std::vector<Index>& shape) {
      super_type::shape_ = shape;
      super_type::layer_state_ = shape;
      super_type::back_integrator_ = nullptr;
      super_type::self_integrator_ = nullptr;
      super_type::activation_function_ = nullptr;
      super_type::parameter_count_ = 0;
    }

    InputLayer(const std::initializer_list<Index>& shape) {
      super_type::shape_ = shape;
      super_type::layer_state_ = shape;
      super_type::back_integrator_ = nullptr;
      super_type::self_integrator_ = nullptr;
      super_type::activation_function_ = nullptr;
      super_type::parameter_count_ = 0;
    }

    virtual void operator()(const Layer<TReal>* prev_layer) {}

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

    virtual void Reset() { super_type::layer_state_.Fill(0.0); }

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

template<typename TReal>
class MotorLayer : public Layer<TReal> {
  public:
    typedef Layer<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef typename super_type::Factor Factor;

    MotorLayer() : super_type() {
    }

    MotorLayer(Index num_outputs, Index num_inputs,
        Activator<TReal>* activation_function) {
      super_type::activation_function_ = activation_function;
      super_type::back_integrator_ = new nervous_system::All2AllIntegrator<TReal>(num_outputs, num_inputs);
      super_type::self_integrator_ = nullptr;
      super_type::parameter_count_ = super_type::activation_function_->GetParameterCount()
                                   + super_type::back_integrator_->GetParameterCount();
      super_type::layer_state_ = multi_array::Tensor<TReal>({num_outputs});
      super_type::input_buffer_ = multi_array::Tensor<TReal>({num_outputs});
    }

    virtual ~MotorLayer() {};

    virtual void operator()(const Layer<TReal>* prev_layer) {
      // First clear input buffer
      super_type::input_buffer_.Fill(0.0);
      // Call back integrator first to resolve input from prev layer
      (*super_type::back_integrator_)(prev_layer->state(), super_type::input_buffer_);
      // Apply activation and update state
      (*super_type::activation_function_)(super_type::layer_state_, super_type::input_buffer_);
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (super_type::parameter_count_ != parameters.size()) {
        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 given");
      }
      // configure back integrator parameters
      super_type::back_integrator_->Configure(
        parameters.slice(0, super_type::back_integrator_->GetParameterCount()));
      // configure activation parameters
      super_type::activation_function_->Configure(
        parameters.slice(parameters.stride() * super_type::back_integrator_->GetParameterCount(),
                         super_type::activation_function_->GetParameterCount()));
    }

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

      // layout produced in configure order: back->act
      Index order = 0;
      std::vector<PARAMETER_TYPE> back_layout = super_type::back_integrator_->GetParameterLayout();
      for (auto par_type_ptr = back_layout.begin();
          par_type_ptr != back_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      std::vector<PARAMETER_TYPE> act_layout = super_type::activation_function_->GetParameterLayout();
      for (auto par_type_ptr = act_layout.begin();
          par_type_ptr != act_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      return layout;
    }
};

/*
 * A motor layer that has no internal memory, and whose outputs represents
 * a distribution, so that all state members sum to 1.
 */
template<typename TReal>
class SoftMaxMotorLayer : public MotorLayer<TReal> {
  public:
    typedef MotorLayer<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef typename super_type::Factor Factor;

    SoftMaxMotorLayer() : super_type() {
    }

    SoftMaxMotorLayer(Index num_outputs, Index num_inputs, TReal temperature) {
      super_type::activation_function_ = new nervous_system::SoftMaxActivator<TReal>(temperature);
      super_type::back_integrator_ = new nervous_system::All2AllIntegrator<TReal>(num_outputs, num_inputs);
      super_type::self_integrator_ = nullptr;
      super_type::parameter_count_ = super_type::activation_function_->GetParameterCount()
                                     + super_type::back_integrator_->GetParameterCount();
      super_type::layer_state_ = multi_array::Tensor<TReal>({num_outputs});
      super_type::input_buffer_ = multi_array::Tensor<TReal>({num_outputs});
    }
};

/*
 * A motor layer that uses the Eigen A2A integrator.
 */
template<typename TReal>
class EigenMotorLayer : public MotorLayer<TReal> {
  public:
    typedef MotorLayer<TReal> super_type;
    typedef typename super_type::Index Index;
    typedef typename super_type::Factor Factor;

    EigenMotorLayer() : super_type() {
    }

    EigenMotorLayer(Index num_outputs, Index num_inputs,
                    Activator<TReal>* activation_function) {
      super_type::activation_function_ = activation_function;
      super_type::back_integrator_ = new nervous_system::All2AllEigenIntegrator<TReal>(num_outputs,
                                                                                num_inputs);
      super_type::self_integrator_ = nullptr;
      super_type::parameter_count_ = super_type::activation_function_->GetParameterCount()
                                     + super_type::back_integrator_->GetParameterCount();
      super_type::layer_state_ = multi_array::Tensor<TReal>({num_outputs});
      super_type::input_buffer_ = multi_array::Tensor<TReal>({num_outputs});
    }
};

template<typename TReal>
class RewardModulatedMotorLayer : public RewardModulatedLayer<TReal>
{
  public:
    using super_type = RewardModulatedLayer<TReal>;
    using Index = typename super_type::Index;

    RewardModulatedMotorLayer(Index num_outputs,
                              Index num_inputs,
                              Activator<TReal>* activation_function,
                              TReal reward_smoothing_factor,
                              TReal activation_smoothing_factor,
                              TReal learning_rate)
      : super_type({num_outputs},
                   new nervous_system::RewardModulatedAll2AllIntegrator<TReal>(num_outputs,
                                                                               num_inputs,
                                                                               learning_rate),
                   nullptr, activation_function, reward_smoothing_factor,
                   activation_smoothing_factor)
    {
    }

    virtual void operator()(const Layer<TReal>* prev_layer) {
      // First clear input buffer
      super_type::input_buffer_.Fill(0.0);
      // Call back integrator first to resolve input from prev layer
      (*super_type::back_integrator_)(prev_layer->state(), super_type::input_buffer_);
      // Apply activation and update state
      (*super_type::activation_function_)(super_type::layer_state_, super_type::input_buffer_);
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (super_type::parameter_count_ != parameters.size()) {
        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 given");
      }
      // configure back integrator parameters
      super_type::back_integrator_->Configure(
          parameters.slice(0, super_type::back_integrator_->GetParameterCount()));
      // configure activation parameters
      super_type::activation_function_->Configure(
          parameters.slice(parameters.stride()
                           * super_type::back_integrator_->GetParameterCount(),
                           super_type::activation_function_->GetParameterCount()));
    }

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

      // layout produced in configure order: back->act
      Index order = 0;
      std::vector<PARAMETER_TYPE> back_layout = super_type::back_integrator_->GetParameterLayout();
      for (auto par_type_ptr = back_layout.begin();
           par_type_ptr != back_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      std::vector<PARAMETER_TYPE> act_layout = super_type::activation_function_->GetParameterLayout();
      for (auto par_type_ptr = act_layout.begin();
           par_type_ptr != act_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      return layout;
    }

    virtual void UpdateWeights(const TReal reward, const Layer<TReal>* prev_layer) {
      // call weight update function
      dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::back_integrator_)->UpdateWeights(
          reward, super_type::reward_average_, prev_layer->state(), super_type::input_buffer_,
          super_type::activation_averages_);

      // update rolling avgerages
      super_type::reward_average_ =
          utilities::ExponentialRollingAverage(reward,
                                               super_type::reward_average_,
                                               super_type::reward_smoothing_factor_);
      for (Index i = 0; i < super_type::activation_averages_.size(); ++i) {
        super_type::activation_averages_[i] =
            utilities::ExponentialRollingAverage(super_type::input_buffer_[i],
                                                 super_type::activation_averages_[i],
                                                 super_type::activation_smoothing_factor_);
      }
    }
};

template<typename TReal>
class NoisyRewardModulatedMotorLayer : public NoisyRewardModulatedLayer<TReal>
{
  public:
    using super_type = NoisyRewardModulatedLayer<TReal>;
    using Index = typename super_type::Index;

    NoisyRewardModulatedMotorLayer(Index num_outputs,
                              Index num_inputs,
                              Activator<TReal>* activation_function,
                              TReal reward_smoothing_factor,
                              TReal activation_smoothing_factor,
                              const TReal standard_deviation,
                              const std::uint64_t seed,
                              TReal learning_rate)
    : super_type({num_outputs},
                 new nervous_system::RewardModulatedAll2AllIntegrator<TReal>(num_outputs,
                                                                             num_inputs,
                                                                             learning_rate),
                 nullptr, activation_function, reward_smoothing_factor,
                 activation_smoothing_factor,
                 standard_deviation, seed)
    {
    }

    virtual void operator()(const Layer<TReal>* prev_layer) {
      // First clear input buffer
      super_type::input_buffer_.Fill(0.0);
      // Call back integrator first to resolve input from prev layer
      (*super_type::back_integrator_)(prev_layer->state(), super_type::input_buffer_);
      // Apply activation and update state
      super_type::ApplyNoise(super_type::input_buffer_);
      (*super_type::activation_function_)(super_type::layer_state_,
                                          super_type::input_buffer_);
    }

    virtual void Configure(const multi_array::ConstArraySlice<TReal>& parameters) {
      if (super_type::parameter_count_ != parameters.size()) {
        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 given");
      }
      // configure back integrator parameters
      super_type::back_integrator_->Configure(
      parameters.slice(0, super_type::back_integrator_->GetParameterCount()));
      // configure activation parameters
      super_type::activation_function_->Configure(
      parameters.slice(parameters.stride()
                       * super_type::back_integrator_->GetParameterCount(),
                       super_type::activation_function_->GetParameterCount()));
    }

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

      // layout produced in configure order: back->act
      Index order = 0;
      std::vector<PARAMETER_TYPE> back_layout = super_type::back_integrator_->GetParameterLayout();
      for (auto par_type_ptr = back_layout.begin();
           par_type_ptr != back_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      std::vector<PARAMETER_TYPE> act_layout = super_type::activation_function_->GetParameterLayout();
      for (auto par_type_ptr = act_layout.begin();
           par_type_ptr != act_layout.end(); ++par_type_ptr) {
        layout[order] = *par_type_ptr;
        ++order;
      }

      return layout;
    }

    virtual void UpdateWeights(const TReal reward, const Layer<TReal>* prev_layer) {
      // call weight update function
      dynamic_cast<RewardModulatedIntegrator<TReal>*>(super_type::back_integrator_)->UpdateWeights(
        reward, super_type::reward_average_, prev_layer->state(), super_type::input_buffer_,
        super_type::activation_averages_);

      // update rolling avgerages
      super_type::reward_average_ =
        utilities::ExponentialRollingAverage(reward,
                                           super_type::reward_average_,
                                           super_type::reward_smoothing_factor_);
      for (Index i = 0; i < super_type::activation_averages_.size(); ++i) {
        super_type::activation_averages_[i] =
        utilities::ExponentialRollingAverage(super_type::input_buffer_[i],
                                             super_type::activation_averages_[i],
                                             super_type::activation_smoothing_factor_);
      }
    }
};

/*
 * Calculates the number of links the integrator has
 */
template <typename Factor, typename TReal>
Factor GetNumOfLinks(const Integrator<TReal>* integrator) {

  if (integrator == nullptr) {
    return 0; // If there is no integrator (which is valid) it has no links.
  }

  INTEGRATOR_TYPE integrator_type = integrator->GetIntegratorType();
  Factor num_links = 0;
  switch (integrator_type) {
    case ALL2ALL_INTEGRATOR: {
      num_links = integrator->GetParameterCount();
      break;
    }

    case NONE_INTEGRATOR: {
      break;
    }

    case CONV_INTEGRATOR: {
      const Conv2DIntegrator<TReal> *conv_integrator = dynamic_cast<const Conv2DIntegrator<TReal> *>(integrator);
      const multi_array::Array<std::size_t, 3> filter = conv_integrator->GetFilterShape();
      num_links = filter[0] * filter[1] * filter[2] * conv_integrator->GetMinTarSize();
      break;
    }

    case TRUNCATED_RECURRENT_INTEGRATOR:
    case RECURRENT_INTEGRATOR: {
      const RecurrentIntegrator<TReal> *recurrent_integrator = dynamic_cast<const RecurrentIntegrator<TReal> *>(integrator);
      const graphs::PredecessorGraph<> graph = recurrent_integrator->GetGraph();
      num_links = graph.NumEdges();
      break;
    }

    case RESERVOIR_INTEGRATOR: {
      const ReservoirIntegrator<TReal> *reservoir_integrator = dynamic_cast<const ReservoirIntegrator<TReal> *>(integrator);
      const graphs::PredecessorGraph<TReal> graph = reservoir_integrator->GetGraph();
      num_links = graph.NumEdges();
      break;
    }

    default:
      throw std::invalid_argument("Error: Integrator type not found.");
  }

  return num_links;
};

/*
 * Calculates the average degree by combining information from the back and
 * self integrators, and then assigns the values to the respective links
 */
template <typename Factor, typename TReal>
void EvaluateNormalizationFactors(std::vector<Factor>& normalization_factors,
                                  const Integrator<TReal>* back_integrator,
                                  const Integrator<TReal>* self_integrator,
                                  std::size_t layer_size) {

  std::size_t num_back_links = 0;
  std::pair<std::size_t, std::size_t> back_weight_slice{0,0};
  if (back_integrator != nullptr) {
    num_back_links = GetNumOfLinks<std::size_t, TReal>(back_integrator);
    back_weight_slice = back_integrator->GetWeightIndexRange();
  }

  std::size_t num_self_links = 0;
  std::pair<std::size_t, std::size_t> self_weight_slice{0,0};
  if (self_integrator != nullptr) {
    num_self_links = GetNumOfLinks<std::size_t, TReal>(self_integrator);
    self_weight_slice = self_integrator->GetWeightIndexRange();
  }

  // Because self params are added after back params, we have to offset the weight indices for self
  if (back_integrator != nullptr) {
    self_weight_slice.first += back_integrator->GetParameterCount();
    self_weight_slice.second += back_integrator->GetParameterCount();
  }

  // Calculate average degree and the assign it to the weight indices of self and back
  Factor average_degree = (num_back_links + num_self_links) / static_cast<double>(layer_size);
  for (std::size_t iii = 0; iii < normalization_factors.size(); ++iii) {
    if (((iii >= back_weight_slice.first) && (iii < back_weight_slice.second)) ||
       ((iii >= self_weight_slice.first) && (iii < self_weight_slice.second))) {
      normalization_factors[iii] = average_degree;
    }
  }
};

} // End nervous_system namespace

#endif /* NN_LAYER_H_ */


#include <gtest/gtest.h>

#include "scheme/nest/NEST.hh"
#include "scheme/nest/pmap/EulerAnglesMap.hh"
#include "scheme/io/dump_pdb_atom.hh"

#include <Eigen/Geometry>

#include <random>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>

#include <fstream>

namespace scheme { namespace nest { namespace pmap { namespace test {

using std::cout;
using std::endl;

TEST( EulerAnglesMap, covering ){
	using namespace Eigen;
	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	cout << "EulerAnglesMap Covrad" << endl;
	int NRES = 7;
	// int const NITER = 1000000;
	int NITER = 100000;
	NEST<3,Matrix3d,EulerAnglesMap> nest;
	for(int r = 1; r <= NRES; ++r){
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i < NITER; ++i){
			Eigen::Quaterniond q( fabs(gauss(rng)), gauss(rng), gauss(rng), gauss(rng) );
			q.normalize();
			Matrix3d m = nest.set_and_get( nest.get_index(q.matrix(),r) , r );
			Quaterniond qcen(m);
			// if( q.angularDistance(qcen) > maxdiff ){
			// 	RowVector3d euler; numeric::euler_angles(q.matrix(),euler);
			// 	euler[0] /= M_PI*2.0;
			// 	euler[1] /= M_PI*2.0;
			// 	euler[2] /= M_PI;
			// 	cout << r << " " << maxdiff << " " << euler << endl;
			// }
			avgdiff += q.angularDistance(qcen);
			maxdiff = std::max(maxdiff,q.angularDistance(qcen));
		}
		avgdiff /= NITER;
		// size/2 because half samples are ignored
		double volfrac = (double)nest.size(r)/2*(maxdiff*maxdiff*maxdiff)*4.0/3.0*M_PI / 8.0 / M_PI / M_PI;
		double avgfrac = (double)nest.size(r)/2*(avgdiff*avgdiff*avgdiff)*4.0/3.0*M_PI / 8.0 / M_PI / M_PI;
		printf("%2i %16llu %10.5f %10.5f %10.5f %10.5f %10.5f\n",
			r, nest.size(r)/2, maxdiff*180.0/M_PI, avgdiff*180.0/M_PI, maxdiff/avgdiff, volfrac, avgfrac );
		// cout << boost::format("%2i %20i %.7d %.7d") % r % nest.size(r) % (maxdiff*180.0/M_PI) % volfrac << endl;
	}

}

TEST( EulerAnglesMap, shapes ){
	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	/// inspect maxdiff/avgdiff for some simple shapes
	int NITER = 10000;
	{
		util::SimpleArray<3,double> b(1,1,1);
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i < NITER; ++i){
			util::SimpleArray<3,double> samp(uniform(rng),uniform(rng),uniform(rng));
			samp = (samp-0.5) * b;
			if(samp.norm() > 0.5){ --i; continue; }
			avgdiff += samp.norm();
			maxdiff = std::max(samp.norm(),maxdiff);
		}
		avgdiff /= NITER;
		cout << "sphere: " << maxdiff / avgdiff << endl;
	}
	{
		util::SimpleArray<3,double> b(1,1,1);
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i < NITER; ++i){
			util::SimpleArray<3,double> samp(uniform(rng),uniform(rng),uniform(rng));
			samp = (samp-0.5) * b;
			avgdiff += samp.norm();
			maxdiff = std::max(samp.norm(),maxdiff);
		}
		avgdiff /= NITER;
		cout << "square: " << maxdiff / avgdiff << endl;
	}
	{
		util::SimpleArray<3,double> b(2,1,1);
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i < NITER; ++i){
			util::SimpleArray<3,double> samp(uniform(rng),uniform(rng),uniform(rng));
			samp = (samp-0.5) * b;
			avgdiff += samp.norm();
			maxdiff = std::max(samp.norm(),maxdiff);
		}
		avgdiff /= NITER;
		cout << "rect211: " << maxdiff / avgdiff << endl;
	}
	{
		util::SimpleArray<3,double> b(1,1,1), cen(0.135022,0.135022,0.135022);
		double maxdiff=0, avgdiff=0;
		for(int i = 0; i < NITER; ++i){
			util::SimpleArray<3,double> samp(uniform(rng),uniform(rng),uniform(rng));
			samp = (samp-0.5) * b;
			if( samp.sum() < 0 ){ --i; continue; }
			avgdiff += (samp-cen).norm();
			maxdiff = std::max((samp-cen).norm(),maxdiff);
		}
		avgdiff /= NITER;
		cout << "triang: " << maxdiff / avgdiff << endl;
	}

}

// TEST( EulerAnglesMap, visualize ){
// 	using namespace Eigen;

// 	std::mt19937 rng((unsigned int)time(0));
// 	std::normal_distribution<> gauss;
// 	std::uniform_real_distribution<> uniform;

// 	Quaterniond qrand( gauss(rng), gauss(rng), gauss(rng), gauss(rng) );
// 	// Quaterniond qrand( 1,0,0,0 );
// 	qrand.normalize();
// 	Vector3d X = qrand*Vector3d(1,0  ,0  );
// 	Vector3d Y = qrand*Vector3d(0,1.2,0  );
// 	Vector3d Z = qrand*Vector3d(0,0  ,1.4);

// 	NEST<3,Matrix3d,EulerAnglesMap> nest;
// 	// size_t beg = 0;
// 	// while(!nest.set_state(beg,10)) beg = std::max<size_t>(uniform(rng)*(nest.size(10)-1000),0);

// 	for(size_t r = 0; r <= 8; ++r){
// 		int N = 8*8*8;
// 		// int beg = std::max( 0, (int)nest.size(r)/12 - N/2 );
// 		int beg = 0;
// 		std::ofstream out(("euler_"+boost::lexical_cast<std::string>(r)+".pdb").c_str());
// 		io::dump_pdb_atom(out,  "Z" ,0,Vector3d(0,0,0));
// 		int count1 = 0, count2 = 0;
// 		// cout << r << " " << nest.size(r) << " " << (beg>>(4*(10-r))) << endl;
// 		// continue;
// 		// for(size_t i = beg>>(4*(10-r)); i < nest.size(r); ++i){
// 		for(size_t i = beg; i < nest.size(r); ++i){
// 			++count1;
// 			if( nest.set_state(i,r) ){
// 				++count2;
// 				if( count1 > N) break;
// 				Matrix3d m = nest.value();
// 				// cout << r << " " << i << " " << q.coeffs().transpose() << endl;
// 				Vector3d ximg = m * X;
// 				Vector3d yimg = m * Y;
// 				Vector3d zimg = m * Z;;
// 				// cout << r << " " << nest.cell_index(i,r) << endl;
// 				// out << "MODEL" << std::endl;
// 				// io::dump_pdb_atom(out,  "Z" ,count2,Vector3d(0,0,0));
// 				io::dump_pdb_atom(out, "O" ,count2,50*ximg);
// 				io::dump_pdb_atom(out, "NI",count2,50*yimg);
// 				io::dump_pdb_atom(out, "N" ,count2,50*zimg);
// 				// out << "ENDMDL" << std::endl;
// 			}
// 		}
// 		out.close();
// 		cout << r << " " << count2 / (double)count1 << endl;
// 	}

// }

}}}}


#include <gtest/gtest.h>

#include "scheme/actor/Atom.hh"
#include "scheme/chemical/ligand_factory.hh"
#include "scheme/numeric/euler_angles.hh"

#include <Eigen/Geometry>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

#include <random>

#include <fstream>

namespace scheme {

using std::cout;
using std::endl;

template<class T>
std::string str(T const & t){ return boost::lexical_cast<std::string>(t); }

template<class M,class T>
std::vector<T> operator*(M const & m, std::vector<T> const & v){
	std::vector<T> r(v);
	BOOST_FOREACH(T & t,r) t = T(t,m);
	return r;
}

template<class T>
void dump_pdb(std::vector<T> const & v,std::string fname){
	std::ofstream out(fname.c_str());
	BOOST_FOREACH(T const & t,v) io::dump_pdb_atom(out,t);
	out.close();
}

TEST( hbond_5dof, DISABLED_test ){
	using namespace Eigen;
	typedef Transform<double,3,AffineCompact> Xform;
	typedef Vector3d Position;
	typedef actor::Atom<Position> Atom;
	chemical::LigandFactory<Atom> f;

	std::vector<Atom> gly,trp,trp1;
	f.make_atoms( std::back_inserter(trp), "TRP", true );
	f.make_atoms( std::back_inserter(gly), "GLY", true );
	trp = AngleAxisd( 0.5*M_PI,Vector3d::UnitY()) * trp;
	gly = AngleAxisd(-0.5*M_PI,Vector3d::UnitY()) * gly;

	 // -1.109  -1.823  -2.927
	 // -2.113  -1.882  -3.017

	std::mt19937 rng((unsigned int)time(0));
	std::normal_distribution<> gauss;
	std::uniform_real_distribution<> uniform;

	Eigen::Quaterniond q( fabs(gauss(rng)), gauss(rng), gauss(rng), gauss(rng) );
	q.normalize();
	Xform frame1 = Xform::Identity();//( q.matrix() ); frame1.translation() = Vector3d(0,0,2);
	// frame1.makeAffine();

	dump_pdb(frame1*gly,"test1.pdb");
	double e0 = 0;//uniform(rng)*2.0*M_PI;
	double e2 = 0;//uniform(rng)*1.0*M_PI;
	for(double e1 = 0; e1 < 2*M_PI; e1+=M_PI/8){

		Matrix3d r; numeric::from_euler_angles(Vector3d(e0,e1,e2),r);
		Xform x(r);
		x.translation() = Vector3d(-3.0,-1.88,4);
		x = x.inverse();
		x = frame1*x;

		trp1 = x*trp;
		dump_pdb(trp1,"test"+str(e1)+".pdb");

		Matrix3d m = ( x.inverse()*frame1 ).linear();
		Vector3d euler; numeric::euler_angles( m, euler );
		printf("%7.4f %7.4f %7.4f\n",euler[0],euler[1],euler[2]);

	}

}

}


//==================================================================================================
/**

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SINHC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SINHC_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/arch/common/detail/generic/sinhc_kernel.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/unary_minus.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( sinhc_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        //////////////////////////////////////////////////////////////////////////////
        // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
        // respectively rational(double) approx inspired from cephes sinh approx.
        // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
        // computed
        // * in the first case sinh is ((e-rec(e))/2)/x
        // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
        // Threshold is Maxlog - Log_2
        //////////////////////////////////////////////////////////////////////////////
        A0 x = bs::abs(a0);
        auto lt1= is_less(x, One<A0>());
        std::size_t nb = nbtrue(lt1);
        A0 z = Zero<A0>();
        if( nb > 0)
        {
          z = detail::sinhc_kernel<A0>::compute(sqr(x));
          if(nb >=A0::static_size) return z;
        }
        auto test1 = is_greater(x, Maxlog<A0>()-Log_2<A0>());
        A0 fac = if_else(test1, Half<A0>(), One<A0>());
        A0 tmp = exp(x*fac);
        A0 tmp1 = (Half<A0>()*tmp)/x;
        A0 r =  if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
        #ifndef BOOST_SIMD_NO_INFINITIES
        r = if_else(is_equal(x, Inf<A0>()), x, r);
        #endif
        return if_else(lt1, z, r);
      }
   };

} } }

#endif


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef QCTOOL_HAPLOTYPE_FREQUENCY_LOGLIKELIHOOD_HPP
#define QCTOOL_HAPLOTYPE_FREQUENCY_LOGLIKELIHOOD_HPP

#include <vector>
#include <Eigen/Core>
#include <boost/math/distributions/binomial.hpp>

// Loglikelihood of table of genotypes  and known haplotypes at 2 biallelic variants,
// given parameters specifying the haplotype frequencies.
// The parameters are \pi_01, \pi_10, and \pi_11.  Then \pi_00 can be
// computed as one minus the sum of the others.
//
// This loglikelihood can additionally estimate the parameters using the EM algorithm.
//
struct HaplotypeFrequencyLogLikelihood {
	typedef Eigen::MatrixXd Matrix ;
	typedef Eigen::VectorXd Vector ;
	typedef Eigen::RowVectorXd RowVector ;

	HaplotypeFrequencyLogLikelihood(
		Matrix const& genotype_table,
		Matrix const& haplotype_table = Matrix::Zero(2,2)
	) ;

	void evaluate_at( Vector const& pi ) ;
	double get_value_of_function() const ;
	Vector get_value_of_first_derivative() ;
	Matrix get_value_of_second_derivative() ;

	Vector get_MLE_by_EM() const ;

	private:
		Matrix const m_genotype_table ;
		Matrix const m_haplotype_table ;
		std::vector< std::vector< RowVector > > m_dpi ;
		double m_ll ;
		RowVector m_D_ll ;
		Matrix m_DDt_ll ;
	private:
		Vector estimate_parameters( double const p ) const ;
} ;

#endif


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <oret/datapaths.hpp>
#include <oret/toplevelfixture.hpp>

#include <ored/marketdata/fittedbondcurvehelpermarket.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/portfolio.hpp>

#include <ql/math/initializers.hpp>
#include <ql/pricingengines/bond/discountingbondengine.hpp>
#include <ql/termstructures/yield/bondhelpers.hpp>
#include <ql/termstructures/yield/nonlinearfittingmethods.hpp>

using namespace QuantLib;
using namespace ore::data;

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(FittedBondCurveTests)

BOOST_AUTO_TEST_CASE(testCurveFromFixedRateBonds) {

#if QL_HEX_VERSION >= 0x01190000 || defined(QL_ORE_PATCH)
    // will work in QL 1.19
    Date asof(6, April, 2020);
    Settings::instance().evaluationDate() = asof;

    BOOST_TEST_MESSAGE("read pricing engine config");
    auto engineData = boost::make_shared<EngineData>();
    engineData->fromFile(TEST_INPUT_FILE("pricingengine.xml"));

    BOOST_TEST_MESSAGE("read portfolio of bonds");
    auto portfolio = boost::make_shared<Portfolio>();
    portfolio->load(TEST_INPUT_FILE("portfolio1.xml"));

    BOOST_TEST_MESSAGE("build portfolio against FittedBondCurveHelperMarket");
    auto engineFactory =
        boost::make_shared<EngineFactory>(engineData, boost::make_shared<FittedBondCurveHelperMarket>());
    portfolio->build(engineFactory);

    BOOST_TEST_MESSAGE("set up bond helpers");
    std::vector<boost::shared_ptr<Bond>> bonds;
    std::vector<boost::shared_ptr<BondHelper>> helpers;
    for (auto const& t : portfolio->trades()) {
        auto bond = boost::dynamic_pointer_cast<Bond>(t->instrument()->qlInstrument());
        BOOST_REQUIRE(bond);
        bonds.push_back(bond);
        helpers.push_back(boost::make_shared<BondHelper>(Handle<Quote>(boost::make_shared<SimpleQuote>(100.0)), bond));
    }

    BOOST_TEST_MESSAGE("build fitted bond curve");
    Array guess(4);
    guess << 0.03, 0.03, 0.03, 0.5;
    NelsonSiegelFitting method((Array(), boost::shared_ptr<OptimizationMethod>(), Array()));
    auto curve =
        boost::make_shared<FittedBondDiscountCurve>(asof, helpers, Actual365Fixed(), method, 1E-10, 10000, guess);

    BOOST_TEST_MESSAGE("cost = " << curve->fitResults().minimumCostValue());

    auto engine = boost::make_shared<DiscountingBondEngine>(Handle<YieldTermStructure>(curve));
    for (auto const& b : bonds) {
        b->setPricingEngine(engine);
        BOOST_TEST_MESSAGE("bond helper maturity " << b->maturityDate() << " has clean price " << b->cleanPrice()
                                                   << " discount factor is " << curve->discount(b->maturityDate()));
        BOOST_CHECK_CLOSE(b->cleanPrice(), 100.0, 0.01); // 1bp tolerance in absolute price
    }
#endif
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>

#include <Eigen/Dense>

#include <windows.h>
#include <GL/glew.h>
#include <GLFW/glfw3.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include "Texture.h"
#include "PointCloudRenderer.h"

using namespace std;
using namespace Eigen;
using namespace glm;

glm::vec3 position = glm::vec3(0, 0, 0);
float horizontalAngle = 0.0f;
float verticalAngle = 0.0f;

// Initial
float fovy = 60.0f;
float aspect = 1.0f;
float gNear = 0.01f;
float gFar = 1000.0f;
Eigen::Vector3f pos_vec;
Eigen::Vector3f focal_vec;
Eigen::Vector3f up_vec;
glm::mat4 MVP_original;

float speed = 1.0f;
float mouseSpeed = 0.002f;

void controlPointClouds(GLFWwindow* currentWindow, const int &window_width, const int &window_height, glm::mat4 &projectionMatrix, glm::mat4 &viewMatrix)
{
	static double lastTime = glfwGetTime();
	double currentTime = glfwGetTime();
	float deltaTime = float(currentTime - lastTime);

	double xpos, ypos;
	glfwGetCursorPos(currentWindow, &xpos, &ypos);
	glfwSetCursorPos(currentWindow, window_width / 2, window_height / 2);

	horizontalAngle += mouseSpeed * float(window_width / 2 - xpos);
	verticalAngle += mouseSpeed * float(window_height / 2 - ypos);

	glm::vec3 direction(
		cos(verticalAngle) * sin(horizontalAngle),
		sin(verticalAngle),
		cos(verticalAngle) * cos(horizontalAngle)
	);

	glm::vec3 right = glm::vec3(
		sin(horizontalAngle - 3.14f / 2.0f),
		0,
		cos(horizontalAngle - 3.14f / 2.0f)
	);

	glm::vec3 up = glm::cross(right, direction);

	if (glfwGetKey(currentWindow, GLFW_KEY_UP) == GLFW_PRESS)
	{
		position += direction * deltaTime * speed;
	}

	if (glfwGetKey(currentWindow, GLFW_KEY_DOWN) == GLFW_PRESS)
	{
		position -= direction * deltaTime * speed;
	}

	if (glfwGetKey(currentWindow, GLFW_KEY_RIGHT) == GLFW_PRESS)
	{
		position += right * deltaTime * speed;
	}

	if (glfwGetKey(currentWindow, GLFW_KEY_LEFT) == GLFW_PRESS)
	{
		position -= right * deltaTime * speed;
	}

	projectionMatrix = glm::perspective(glm::radians(fovy), aspect, gNear, gFar);
	viewMatrix = glm::lookAt(position,position + direction,up);

	lastTime = currentTime;
}

void sen::PointCloudRenderer::initializeMVP(const k4a::calibration &calibration)
{
	Eigen::Matrix3f intrinsics;
	Eigen::Matrix4f extrinsics;

	k4a_calibration_intrinsic_parameters_t intrinsics_color = calibration.color_camera_calibration.intrinsics.parameters;

	intrinsics <<
		intrinsics_color.param.fx, 0.f, intrinsics_color.param.cx,
		0.f, intrinsics_color.param.fy, intrinsics_color.param.cy,
		0.f, 0.f, 1.f;

	extrinsics = Eigen::Matrix4f::Identity();

	pos_vec = extrinsics.block<3, 1>(0, 3);
	Eigen::Matrix3f rotation = extrinsics.block<3, 3>(0, 0);
	Eigen::Vector3f y_axis(0.f, 1.f, 0.f);
	up_vec = Eigen::Vector3f(rotation * y_axis);

	Eigen::Vector3f z_axis(0.f, 0.f, 1.f);
	focal_vec = pos_vec + rotation * z_axis;

	Eigen::Vector2i window_size;
	window_size[0] = 2 * static_cast<int> (intrinsics(0, 2));
	window_size[1] = 2 * static_cast<int> (intrinsics(1, 2));

	fovy = 2 * atan(window_size[1] / (2. * intrinsics(1, 1))) * 180.0 / M_PI;

	aspect = (float)window_size[0] / (float)window_size[1];
	position = glm::make_vec3(pos_vec.data());

	ProjectionMatrix = glm::perspective(glm::radians(fovy), aspect, gNear, gFar);

	ViewMatrix = glm::lookAt(
		glm::vec3(pos_vec[0], pos_vec[1], pos_vec[2]),
		glm::vec3(focal_vec[0], focal_vec[1], focal_vec[2]),
		glm::vec3(up_vec[0], up_vec[1], up_vec[2]));

	glm::mat4 Model = mat4(1.0f);
	MVP = ProjectionMatrix * ViewMatrix * Model;
	MVP_original = MVP;
}

bool sen::PointCloudRenderer::setup(const std::string &vertex_shader_file, const std::string &fragment_shader_file, // => shader file path
	const int &texture_width, const int &texture_height, // => buffer data size
	const k4a::calibration &calibration// => k4a::transformation => convert depthImage & colorImage into point cloud
	)
{
	programID = LoadShaders(vertex_shader_file, fragment_shader_file);
	MatrixID = glGetUniformLocation(programID, "MVP");

	initializeMVP(calibration);

	transformation = k4a::transformation(calibration);
	int dataSize = texture_width * texture_height;
	glGenBuffers(1, &vertexBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
	glBufferData(GL_ARRAY_BUFFER, dataSize * sizeof(glm::vec3), 0, GL_DYNAMIC_DRAW);

	glGenBuffers(1, &colorBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glBufferData(GL_ARRAY_BUFFER, dataSize * sizeof(glm::vec3), 0, GL_DYNAMIC_DRAW);

	return true;
}

void sen::PointCloudRenderer::convertRGBDepthToPointXYZRGB(const k4a::image &colorImage, const k4a::image &depthImage, vector<glm::vec3> &vertices, vector<glm::vec3> &colors)
{
	int color_image_width_pixels = colorImage.get_width_pixels();
	int color_image_height_pixels = colorImage.get_height_pixels();

	k4a::image transformed_depth_image = NULL;
	transformed_depth_image = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
		color_image_width_pixels,
		color_image_height_pixels,
		color_image_width_pixels * (int)sizeof(uint16_t));

	k4a::image point_cloud_image = NULL;
	point_cloud_image = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
		color_image_width_pixels,
		color_image_height_pixels,
		color_image_width_pixels * 3 * (int)sizeof(int16_t));

	transformation.depth_image_to_color_camera(depthImage, &transformed_depth_image);
	transformation.depth_image_to_point_cloud(transformed_depth_image, K4A_CALIBRATION_TYPE_COLOR, &point_cloud_image);

	int width = colorImage.get_width_pixels();
	int height = colorImage.get_height_pixels();

	int16_t *point_cloud_image_data = (int16_t *)(void *)point_cloud_image.get_buffer();
	const uint8_t *color_image_data = colorImage.get_buffer();

	Eigen::Matrix3f m;
	m = Eigen::AngleAxisf(-M_PI, Eigen::Vector3f::UnitZ());

	vertices.resize(width * height);
	colors.resize(width * height);

	for (int i = 0; i < width * height; ++i)
	{
		Vector3f xyz = Vector3f(point_cloud_image_data[3 * i + 0] / 1000.0f,
			point_cloud_image_data[3 * i + 1] / 1000.0f,
			point_cloud_image_data[3 * i + 2] / 1000.0f);

		if (xyz(2) == 0.0f)
		{
			continue;
		}

		xyz = m * xyz;

		Vector3f rgb = Vector3f(color_image_data[4 * i + 2] / 256.0f,
			color_image_data[4 * i + 1] / 256.0f,
			color_image_data[4 * i + 0] / 256.0f);

		vertices[i] = glm::make_vec3(xyz.data());
		colors[i] = glm::make_vec3(rgb.data());
	}
}
void sen::PointCloudRenderer::render(GLFWwindow* currentWindow, const k4a::image &colorImage, const k4a::image &depthImage)
{
	int color_image_width_pixels = colorImage.get_width_pixels();
	int color_image_height_pixels = colorImage.get_height_pixels();

	vector<glm::vec3> vertices;
	vector<glm::vec3> colors;

	convertRGBDepthToPointXYZRGB(colorImage, depthImage, vertices, colors);

	glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
	glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(glm::vec3), &vertices[0], GL_DYNAMIC_DRAW);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glBufferData(GL_ARRAY_BUFFER, colors.size() * sizeof(glm::vec3), &colors[0], GL_DYNAMIC_DRAW);

	glUseProgram(programID);
	if (glfwGetMouseButton(currentWindow, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS)
	{
		controlPointClouds(currentWindow, color_image_width_pixels, color_image_height_pixels, ProjectionMatrix, ViewMatrix);
		glm::mat4 Model = mat4(1.0f);
		MVP = ProjectionMatrix * ViewMatrix * Model;
	}
	else
	{
		MVP = MVP_original;
	}

	glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MVP[0][0]);

	glEnableVertexAttribArray(0);
	glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
	glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);

	glEnableVertexAttribArray(1);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);

	glEnable(GL_PROGRAM_POINT_SIZE);
	glDrawArrays(GL_POINTS, 0, static_cast<int>(color_image_width_pixels * color_image_height_pixels) * 3);

	glDisableVertexAttribArray(0);
	glDisableVertexAttribArray(1);
}
void sen::PointCloudRenderer::deleteBuffer()
{
	glDeleteBuffers(1, &vertexBuffer);
	glDeleteBuffers(1, &colorBuffer);
	glDeleteProgram(programID);
}


#include <vector>
#include <math.h>
#include <iostream>
#include <string>
#include <sstream>

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

#include <common/Factor.h>
#include <common/Keyframe.h>
#include <common/ClosestKeyframe.h>
#include <common/LastKeyframe.h>
#include <common/Registration.h>
#include <common/Pose2DWithCovariance.h>
#include <common/OdometryBuffer.h>

#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>


//Localization.hpp
// Created on: Apr 20, 2016
//     Author: Hafez Farazi <farazi@ais.uni-bonn.de>
#pragma once
#include <opencv2/opencv.hpp>
#include <math.h>
#include <vision_module/Tools/Parameters.hpp>
#include <vision_module/Tools/General.hpp>
#include <vision_module/Tools/SortFuntions.hpp>
#include <vision_module/Tools/LinearBoundaryChecker.hpp>
#include <vision_module/Tools/LineSegment.hpp>
#include <vision_module/Projections/CameraProjections.hpp>
#include <vision_module/Tools/Kalman.hpp>
#include <vision_module/SoccerObjects/ObstacleDetector.hpp>
#include <gait_msgs/GaitOdom.h>
#include <boost/timer/timer.hpp>
#include <algorithm>    // std::sort
#include <field_model/field_model.h>
#include <tf/transform_broadcaster.h>
#include <math.h>

#include <Eigen/StdVector>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/hyper_graph.h>
#include <g2o/core/solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/types/slam2d/edge_se2.h>
#include <g2o/types/slam2d/vertex_se2.h>

using namespace cv;
using namespace Eigen;
using namespace g2o;

enum LineType
{
	HorUndef,
	HorCenter,
	HorGoal,
	HorGoalNear,
	VerUndef,
	VerLeft,
	VerRight,
	VerLeftNear,
	VerRightNear,
	VerLeftT,
	VerRightT,
	LTRES
};

enum LandmarkType
{
	CenterL = 0, RightL, FrontL, LeftL, BackL
};

const std::string LineTypeName[LTRES] =
{ "HorUndef", "HorCenter", "HorGoal", "HorGoalNear", "VerUndef", "VerLeft",
		"VerRight", "VerLeftNear", "VerRightNear", "VerLeftT", "VerRightT" };
class LineContainer
{
public:
	inline LineContainer(LineSegment _lineTransformed, LineType _type) :
			lineTransformed(_lineTransformed), type(_type)
	{

	}
	LineSegment lineTransformed;
	LineType type;
};

class FeatureContainer
{
public:
	inline FeatureContainer(Point2f _position, string _type) :
			position(_position), type(_type)
	{

	}
	Point2f position;
	string type;
};
/**
* @ingroup VisionModule
*
* @brief For localization in the soccer field
**/
class Localization
{
private:
	inline double getUpdateCoef(double coef, cv::Point2f point)
	{
		double distance = GetDistance(point);

		if (distance > 8)
			return 0;
		if (distance < 3)
			return coef;

		return coef * (1 - (distance / 8));
	}

	inline double getUpdateCoef(double coef, LineSegment line)
	{
		return getUpdateCoef(coef,
				line.GetClosestPointOnLineSegment(cv::Point2d(0, 0)))
				* line.getProbability();
	}

	ros::NodeHandle nodeHandle;
	ros::Subscriber setLoc_sub;
	ros::Subscriber robotTracker_sub;
	ros::Subscriber robotTracker2_sub;
	ros::Subscriber odom_sub;
	Point2d location;
	Point2d locationKalman;
	KalmanFilterC kalmanI;
	CameraProjections *_cameraProjections;
	geometry_msgs::Point robotTrackerLoc;
	float lastOdomX, lastOdomY;
	Point3d globalPos;
	Point3d lastOdom;
	uint32_t lastOdomID;
	Point2f ballPos;
	const field_model::FieldModel* const m_field;
	// TF transforms
	tf::TransformBroadcaster tfBroadcaster;
	tf::StampedTransform tfLocField;
	vector<ObstacleC> *obstacles;
	BlockSolverX::LinearSolverType * linearSolver;
	BlockSolverX* blockSolver;
	OptimizationAlgorithmLevenberg* optimizationAlgorithm;
	int CurrentVertexId;
	int PreviousVertexId;
	int LandmarkCount;
	Point2d odomLastGet;
	bool atLeastOneObservation;
	ros::Time lastSavedNodeTime;
	long unsigned int nodeCounter;
public:
	SparseOptimizer optimizer;

	inline ~Localization()
	{
		optimizer.clear();
		delete optimizationAlgorithm;
	}

	inline Localization(vector<ObstacleC> *obstacles) :
			location(-1.42, 0), locationKalman(location), kalmanI(
					locationKalman), _cameraProjections(
			NULL), lastOdomX(0), lastOdomY(0), globalPos(0, 0, 0), lastOdom(0,
					0, 0), lastOdomID(0), ballPos(0, 0), m_field(
					field_model::FieldModel::getInstance()), obstacles(
					obstacles), CurrentVertexId(0), PreviousVertexId(0), LandmarkCount(
					0), odomLastGet(0, 0), atLeastOneObservation(false), nodeCounter(
					0)
	{

		odom_sub = nodeHandle.subscribe("/gait/odometry", 10,
				&Localization::dead_reckoning_callback, this);
		setLoc_sub = nodeHandle.subscribe<geometry_msgs::Point>(
				"/vision/setLocation", 1, &Localization::setloc_callback, this);

		A = m_field->length();
		B = m_field->width();
		E = m_field->goalAreaDepth();
		F = m_field->goalAreaWidth();
		G = m_field->penaltyMarkerDist();
		H = m_field->centerCircleDiameter();
		D = m_field->goalWidth();
		I = m_field->boundary();
		params.ball->radius.set(m_field->ballDiameter() / 2.);
		A2 = A / 2.;
		B2 = B / 2.;
		H2 = H / 2.;
		A2 = A / 2.;
		B2 = B / 2.;
		E2 = E / 2.;
		F2 = F / 2.;
		G2 = G / 2.;
		H2 = H / 2.;
		D2 = D / 2.;
		I2 = I / 2.;

		// TF transforms
		tfLocField.frame_id_ = "/ego_floor";
		tfLocField.child_frame_id_ = "/loc_field";
		tfLocField.setIdentity();

		lastSavedNodeTime = ros::Time::now();
		// create the linear solver
		linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();
		// create the block solver on the top of the linear solver
		blockSolver = new BlockSolverX(linearSolver);
		//create the algorithm to carry out the optimization
		optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);
	}

	inline Point2d getOdometryFromLastGet()
	{
		if (PreviousVertexId > 0) //Not the first iteration
		{
			Point2d res = odomLastGet;
			odomLastGet.x = odomLastGet.y = 0;
			return res;
		}
		else
			return Point2d(0, 0);
	}

	inline void updateVertexIdx()
	{
		if ((ros::Time::now() - lastSavedNodeTime).toSec() >= 0.03)
		{
			nodeCounter++;
			lastSavedNodeTime = ros::Time::now();
			PreviousVertexId = CurrentVertexId;
			CurrentVertexId++;
			if (CurrentVertexId - LandmarkCount >= 100)
			{
				CurrentVertexId = LandmarkCount;
			}

			{
				VertexSE2 * r = new VertexSE2;
				r->setEstimate(Eigen::Vector3d(location.x, location.y, 0));
				r->setFixed(false);
				r->setId(CurrentVertexId);
				if (optimizer.vertex(CurrentVertexId) != NULL)
				{
					optimizer.removeVertex(optimizer.vertex(CurrentVertexId));
				}

				optimizer.addVertex(r);
			}

			{
				EdgeSE2 * e = new EdgeSE2;
				e->vertices()[0] = optimizer.vertex(PreviousVertexId);
				e->vertices()[1] = optimizer.vertex(CurrentVertexId);
				Point2d dead_reck = getOdometryFromLastGet();
				e->setMeasurement(SE2(dead_reck.x, dead_reck.y, 0));
				Matrix3d information;
				information.fill(0.);
				information(0, 0) = 200;
				information(1, 1) = 200;
				information(2, 2) = 1;
				e->setInformation(information);
				optimizer.addEdge(e);
			}
		}
	}
	inline bool addObservation(Point2d observation, double xFasher,
			double yFasher, LandmarkType type)
	{

		{
			EdgeSE2 * e = new EdgeSE2;

			e->vertices()[0] = optimizer.vertex(type);
			e->vertices()[1] = optimizer.vertex(CurrentVertexId);

			switch (type)
			{
			case RightL:
				observation.y += B2;
				break;
			case FrontL:
				observation.x -= A2;
				break;
			case LeftL:
				observation.y -= B2;
				break;
			case BackL:
				observation.x += A2;
				break;
			default:
				break;
			}
			e->setMeasurement(SE2(observation.x, observation.y, 0));
			Matrix3d information;
			information.fill(0.);
			information(0, 0) = xFasher;
			information(1, 1) = yFasher;
			information(2, 2) = 1;
			e->setInformation(information);

			g2o::RobustKernelCauchy* rk = new g2o::RobustKernelCauchy;
			e->setRobustKernel(rk);

			optimizer.addEdge(e);
		}
		atLeastOneObservation = true;
		return true;
	}

	inline Point2f getBall()
	{
		return ballPos;
	}

	inline void setBall(Point2f _in)
	{
		ballPos = _in;
	}

	inline void setloc_callback(const geometry_msgs::Point::ConstPtr& msg)
	{
		ROS_INFO("Set location received.");
		location.x = msg->x;
		location.y = msg->y;
		locationKalman.x = msg->x;
		locationKalman.y = msg->y;

		{
			boundry_n(location.x, -A2 - I, A2 + I);
			boundry_n(location.y, -B2 - I, B2 + I);
			boundry_n(locationKalman.x, -A2 - I, A2 + I);
			boundry_n(locationKalman.y, -B2 - I, B2 + I);
		}
	}

	inline void setremoteloc_callback(
			const geometry_msgs::PointStamped::ConstPtr& msg)
	{
		this->robotTrackerLoc.x = msg->point.x;
		this->robotTrackerLoc.y = msg->point.y;
		this->robotTrackerLoc.z = msg->point.z;
	}

	inline void dead_reckoning_callback(const gait_msgs::GaitOdomConstPtr & msg)
	{
		if (_cameraProjections == NULL)
		{
			return;
		}
		Point3d curOdom;
		curOdom.x = msg->odom2D.x;
		curOdom.y = msg->odom2D.y;
		curOdom.z = msg->odom2D.theta;

		if (lastOdomID == msg->ID)
		{
			Point3d diffOdom = curOdom - lastOdom;
			globalPos += diffOdom;
			Point2d diffOdom2D(diffOdom.x, diffOdom.y);

			Point2d diffOdom2DCancelOdomRotation = RotateAroundPoint(diffOdom2D,
					Radian2Degree(curOdom.z));

			Point2d diffAbsolut = RotateAroundPoint(
					diffOdom2DCancelOdomRotation,
					-Radian2Degree(_cameraProjections->getHeading()));
			location.x += diffAbsolut.x;
			location.y += diffAbsolut.y;
			robotTrackerLoc.x += diffAbsolut.x;
			robotTrackerLoc.y += diffAbsolut.y;
			odomLastGet.x += diffAbsolut.x;
			odomLastGet.y += diffAbsolut.y;

			robotTrackerLoc.z = _cameraProjections->getHeading();
		}

		lastOdom = curOdom;
		lastOdomID = msg->ID;
	}

	inline Point3d GetLocalization()
	{
		Point3d res;
		res.x = location.x;
		res.y = location.y;
		res.z = _cameraProjections->getHeading();
		if (params.loc->useKalman())
		{
			res.x = locationKalman.x;
			res.y = locationKalman.y;
		}
		if (params.loc->forwardRobotTrackerXY())
		{
			res.x = robotTrackerLoc.x;
			res.y = robotTrackerLoc.y;
		}
		if (params.loc->forwardRobotTrackerZ())
		{
			res.z = robotTrackerLoc.z;
		}

		return res;
	}
	void SendTransform(ros::Time now);
	bool Calculate(vector<LineSegment> &, bool circleDetected, const Point2f &,
			const vector<cv::Point2f> &, const Point2d &,
			const vector<Point2f> &, const bool &confiused,
			vector<LineContainer> &, vector<FeatureContainer> &);

	bool Update(CameraProjections &projection);
	bool Init(string rName);
	void InitG2OGraph();

	double A;
	double B;
	double E;
	double F;
	double G;
	double H;
	double D;
	double I;
	double A2;
	double B2;
	double E2;
	double F2;
	double G2;
	double H2;
	double D2;
	double I2;

};


#include <algorithm>
#include <unordered_map>

#include <Eigen/QR>
#include <descriptor-projection/build-projection-matrix.h>
#include <descriptor-projection/descriptor-projection.h>
#include <descriptor-projection/flags.h>

#include <vi-map/vertex.h>

namespace descriptor_projection {

// Compute the covariance of the descriptors, rows are states, columns are
// samples.
void ComputeCovariance(
    const Eigen::MatrixXf& data, Eigen::MatrixXf* covariance) {
  CHECK_NOTNULL(covariance);
  CHECK_GT(data.cols(), 0) << "Data must not be empty!";
  VLOG(4) << "Got " << data.cols()
          << " samples to compute the covariance from.";
  covariance->setZero(data.rows(), data.rows());
  constexpr int kBlockSize = 10000;
  const int num_blocks = data.cols() / kBlockSize + 1;
  for (int i = 0; i < num_blocks; ++i) {
    const int block_start = i * kBlockSize;
    const int block_size =
        std::min<int>((i + 1) * kBlockSize, data.cols()) - block_start;
    const Eigen::Block<const Eigen::MatrixXf>& data_block =
        data.block(0, block_start, data.rows(), block_size);

    const Eigen::MatrixXf centered =
        data_block.colwise() - data_block.rowwise().mean();
    double normalizer = std::max(static_cast<int>(data_block.cols() - 1), 1);
    covariance->noalias() += (centered * centered.adjoint()) / normalizer;
  }
  (*covariance) /= num_blocks;
}

void BuildListOfMatchesAndNonMatches(
    const Eigen::MatrixXf& all_descriptors, const std::vector<Track>& tracks,
    std::vector<descriptor_projection::DescriptorMatch>* matches,
    std::vector<descriptor_projection::DescriptorMatch>* non_matches) {
  CHECK_NOTNULL(matches);
  CHECK_NOTNULL(non_matches);
  for (const Track& track : tracks) {
    // Add pairs of matching descriptors to the list of matches.
    // TODO(slynen): Consider taking random pairs to not under-estimate the
    // variance.
    for (size_t i = 1; i < track.size(); ++i) {
      matches->emplace_back(track[i - 1], track[i]);
    }
  }

  std::random_device device;
  std::mt19937 generator(device());
  std::uniform_int_distribution<> distribution(0, all_descriptors.cols() - 1);

  for (size_t i = 1; i < static_cast<size_t>(all_descriptors.cols()) &&
                     i < matches->size() * 2;
       ++i) {
    unsigned int index_a = distribution(generator);
    unsigned int index_b = distribution(generator);
    if (index_a == index_b) {
      continue;
    }
    non_matches->emplace_back(index_a, index_b);
  }
}

void BuildCovarianceMatricesOfMatchesAndNonMatches(
    unsigned int descriptor_size, const Eigen::MatrixXf& all_descriptors,
    const std::vector<Track>& tracks, unsigned int* sample_size_matches,
    unsigned int* sample_size_non_matches, Eigen::MatrixXf* cov_matches,
    Eigen::MatrixXf* cov_non_matches) {
  CHECK_NOTNULL(sample_size_matches);
  CHECK_NOTNULL(sample_size_non_matches);
  CHECK_NOTNULL(cov_matches);
  CHECK_NOTNULL(cov_non_matches);

  {  // Scope to limit memory usage.
    unsigned int too_short_tracks = 0;
    unsigned int long_enough_tracks = 0;
    constexpr size_t kMinTrackLength = 5;
    size_t number_of_used_tracks = 0;
    Eigen::MatrixXf sumMuMu;
    sumMuMu.setZero(descriptor_size, descriptor_size);

    std::vector<size_t> descriptors_from_tracks;
    descriptors_from_tracks.reserve(500);

    // Centering.
    constexpr int kMaxNumSamples = 50000;
    for (const Track& track : tracks) {
      if (track.size() < kMinTrackLength) {
        ++too_short_tracks;
        continue;
      }
      if (number_of_used_tracks >= kMaxNumSamples) {
        LOG(WARNING) << "Truncated descriptors to " << kMaxNumSamples << ".";
        break;
      }
      ++long_enough_tracks;

      Eigen::Matrix<float, Eigen::Dynamic, 1> mean;
      mean.resize(descriptor_size, Eigen::NoChange);
      mean.setZero();

      for (const size_t& descriptor_idx : track) {
        descriptors_from_tracks.push_back(descriptor_idx);
        mean += all_descriptors.block(0, descriptor_idx, descriptor_size, 1);
      }

      mean /= track.size();
      CHECK_LE(mean.maxCoeff(), 1.0);
      CHECK_GE(mean.minCoeff(), 0.0);

      Eigen::MatrixXf mu_sq_current = (mean * mean.transpose()).eval();

      sumMuMu += mu_sq_current * track.size();
      ++number_of_used_tracks;
    }

    VLOG(3) << "Got " << long_enough_tracks << " tracks out of "
            << tracks.size() << " (dropped " << too_short_tracks
            << " tracks because they were too short)";

    CHECK(!descriptors_from_tracks.empty());

    VLOG(3) << "Computing matches covariance from "
            << descriptors_from_tracks.size()
            << " matches (descriptor size: " << descriptor_size << ")";

    *sample_size_matches = descriptors_from_tracks.size();

    Eigen::MatrixXf matches;
    matches.resize(descriptor_size, descriptors_from_tracks.size());
    matches.setZero();

    int matched_idx = 0;
    for (const size_t& descriptor_idx : descriptors_from_tracks) {
      matches.block(0, matched_idx, descriptor_size, 1) =
          all_descriptors.block(0, descriptor_idx, descriptor_size, 1);
      ++matched_idx;
    }

    CHECK_GT(descriptors_from_tracks.size(), number_of_used_tracks);

    // Covariance computation for matches.
    cov_matches->noalias() =
        (matches * matches.transpose() - sumMuMu) * 2.0 /
        static_cast<float>(
            descriptors_from_tracks.size() - number_of_used_tracks);
  }  // Scope to limit memory usage.

  // Use all descriptors to estimate the non-matching covariance.
  *sample_size_non_matches = all_descriptors.cols();
  // Compute sample covariances non matched descriptors.
  ComputeCovariance(all_descriptors, cov_non_matches);

  *cov_non_matches *= 2.0f;
}

void ComputeProjectionMatrix(
    const Eigen::MatrixXf& cov_matches, const Eigen::MatrixXf& cov_non_matches,
    Eigen::MatrixXf* A) {
  CHECK_NOTNULL(A);

  const int dimensionality = cov_matches.cols();
  A->resize(dimensionality, dimensionality);

  Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_matches, Eigen::ComputeFullV);

  CHECK_NE(svd.singularValues().minCoeff(), 0)
      << "Rank deficiency for matrix"
         " of samples detected. Probably too little matches.";

  Eigen::MatrixXf Av =
      svd.singularValues().cwiseSqrt().cwiseInverse().asDiagonal() *
      svd.matrixV().transpose();

  Eigen::JacobiSVD<Eigen::MatrixXf> svd_d(
      Av * cov_non_matches * Av.transpose(), Eigen::ComputeFullV);

  Eigen::MatrixXf singular_values_sqrt_inv =
      svd_d.singularValues().cwiseSqrt().cwiseInverse().asDiagonal();

  Eigen::MatrixXf eye;
  eye.resize(dimensionality, dimensionality);
  eye.setIdentity();

  *A = (eye - singular_values_sqrt_inv) * svd_d.matrixV().transpose() *
       svd.singularValues().cwiseInverse().asDiagonal() *
       svd.matrixV().transpose();
}
}  // namespace descriptor_projection


#include <boost/test/unit_test.hpp>
#include "obb.h"

BOOST_AUTO_TEST_SUITE(obb)

BOOST_AUTO_TEST_CASE(world_to_local_vector)
{
	math::obb<3> bounds;
	bounds.origin.set(0, 0, 0);
	bounds.tangent.set(1, 2, 3);
	bounds.normal.set(3, 1, 2);
	bounds.binormal.set(2, 3, 1);

	math::vec<3> point, point_local, point_world;

	point.set(100, 50, -2);

	bounds.world_to_local_vector(point_local, point);
	bounds.local_to_world_vector(point_world, point_local);

	BOOST_REQUIRE((point - point_world).length_sq() < math::EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


#include "utils.h"
#include "hard_clustering.h"
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

using namespace std;

CUGMMLite::CUGMMLite(AdaHardCluster& mdl_,
                     unsigned long int dim_)
    : copt::BoundedProblem<double>(2),
      mdl(mdl_),
      dim(dim_),
      k(mdl_.get_k()),
      mu(mdl_.get_mu()),
      m2(mdl_.get_m2()),
      m4(mdl_.get_m4()){
}

double CUGMMLite::value(const copt::Vector<double>& x) {
  // x = <alpha,kappa>
  double obj = 0.0;
  double alpha = x[0];
  double kappa = x[1];
  double var = 0.0;
  for (size_t c = 0; c < k; ++c) {
    var = mdl.variance(mu[c][dim],alpha,dim);
    obj += (pow(m2[c][dim] - kappa*var,2)
      )/(m4[c][dim] - 2*m2[c][dim]*kappa*var + pow(kappa*var,2));
  }
  return obj;
}

void CUGMMLite::gradient(const copt::Vector<double>& x,
                         copt::Vector<double>& grad) {
  double alpha = x[0];
  double kappa = x[1];
  double var = 0.0;
  double diff_var = 0.0;
  grad[0] = 0.0;
  grad[1] = 0.0;
  double coef = 0.0;
  for (size_t c = 0; c < k; ++c){
    var = mdl.variance(mu[c][dim],alpha,dim);
    diff_var = mdl.diff_variance(mu[c][dim],alpha,dim);
    coef = (2*(pow(m2[c][dim],2)-m4[c][dim])*(m2[c][dim] - kappa*var)
      )/pow(m4[c][dim] - 2*m2[c][dim]*kappa*var + pow(kappa*var,2),2);
    grad[0] += coef*kappa*diff_var;
    grad[1] += coef*var;
  }
}

AdaHardCluster::AdaHardCluster(const vector<vector<double>>& data_,
                       const vector<unsigned long int>& label_,
                       unsigned long int max_round_, unsigned long int k_)
    : data(data_), label(label_) {
  max_round = max_round_;
  k = k_;
  nmis.resize(max_round);
  fill(nmis.begin(), nmis.end(), 0);
  logliks.resize(max_round);
  fill(logliks.begin(), logliks.end(), -numeric_limits<double>::max());
  n_samples = data.size();
  n_dims = data[0].size();
  count.resize(k);
  kappa.resize(n_dims);
  kappa_a.resize(n_dims);
  kappa_b.resize(n_dims);
  mu.resize(k);
  mu_a.resize(k);
  mu_b.resize(k);
  m2.resize(k);
  m4.resize(k);
  v.resize(k);
  w_inv.resize(k);
  for (size_t c = 0; c < k; ++c){
    mu[c].resize(n_dims);
    mu_a[c].resize(n_dims);
    mu_b[c].resize(n_dims);
    m2[c].resize(n_dims);
    m4[c].resize(n_dims);
    v[c].resize(n_dims);
    w_inv[c].resize(n_dims);
  }
  asg.resize(n_samples);
  attr_discrete.resize(n_dims);
  attr_positive.resize(n_dims);
  attr_nonnegative.resize(n_dims);

  alpha.resize(n_dims);
  lb.resize(n_dims);
  ub.resize(n_dims);
  for (unsigned long int j = 0; j < n_dims; ++j) {
    attr_discrete[j] = is_discrete(data, j);
    attr_positive[j] = is_positive(data, j);
    attr_nonnegative[j] = attr_positive[j];
    if (!attr_positive[j]) attr_nonnegative[j] = is_nonnegative(data, j);
  }
  for (unsigned long int j = 0; j < n_dims; ++j) {
    if (attr_discrete[j]) {
      if (attr_nonnegative[j]) {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      } else {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      }
    } else {
      if (attr_positive[j]) {
        alpha[j] = 0;
        // lb[j] = -numeric_limits<double>::max();
        lb[j] = -10;
        ub[j] = 2;
      } else if (attr_nonnegative[j]) {
        alpha[j] = 0.5;
        lb[j] = 0;
        ub[j] = 1;
      } else {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      }
    }
  }
}

double AdaHardCluster::distance(const vector<double>& x,
                                const unsigned long int c) {
  double dist = 0.0;
  for (unsigned long int j = 0; j < n_dims; ++j)
    dist += pow(pow(x[j],2) - kappa[j]*v[c][j],2)*w_inv[c][j];
  return dist;
}

double AdaHardCluster::variance(const double x,
                                const double alpha,
                                const unsigned long int dim){
  // return nnc_variance(x, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_variance(x, alpha);
    else
      return rc_variance(x, alpha);
  } else {
    if (attr_nonnegative[dim])
      return nnc_variance(x, alpha);
    else
      return rc_variance(x, alpha);
  }
}

double AdaHardCluster::diff_variance(const double x,
                                 const double alpha,
                                 const unsigned long int dim) {
  // return nnc_diff_variance(x, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_diff_variance(x, alpha);
    else
      return rc_diff_variance(x, alpha);
  } else {
    if (attr_nonnegative[dim])
      return nnc_diff_variance(x, alpha);
    else
      return rc_diff_variance(x, alpha);
  }
}

unsigned long int AdaHardCluster::get_k() { return k; }

unsigned long int AdaHardCluster::get_max_round() { return max_round; }

unsigned long int AdaHardCluster::get_n_dims() { return n_dims; }

unsigned long int AdaHardCluster::get_n_samples() { return n_samples; }

vector<double>& AdaHardCluster::get_kappa() { return kappa; }

vector<double>& AdaHardCluster::get_logliks() { return logliks; }

vector<double>& AdaHardCluster::get_nmis() { return nmis; }

copt::Vector<double>& AdaHardCluster::get_alpha() { return alpha; }

copt::Vector<double>& AdaHardCluster::get_lb() { return lb; }

copt::Vector<double>& AdaHardCluster::get_ub() { return ub; }

const vector<vector<double>>& AdaHardCluster::get_data() { return data; }

vector<vector<double>>& AdaHardCluster::get_mu() { return mu; }

vector<vector<double>>& AdaHardCluster::get_m2() { return m2; }

vector<vector<double>>& AdaHardCluster::get_m4() { return m4; }

void AdaHardCluster::initialize_random() {
  unsigned long int ri;
  for (size_t c = 0; c < k; ++c) {
    ri = rand() % n_samples;
    for (unsigned long int j = 0; j < n_dims; ++j){
      mu[c][j] = data[ri][j];
      mu_a[c][j] = 0;//mu[c][j];
      mu_b[c][j] = 1.0;
    }
  }
  for (unsigned long int j = 0; j < n_dims; ++j){
    kappa[j] = 1.0;
    kappa_a[j] = 1e-9;
    kappa_b[j] = 1.0;
  }
}

void AdaHardCluster::initialize_k_plus_plus() {
  boost::mt19937 gen;
  unsigned long int ri, k_eff;
  ri = rand() % n_samples;
  for (unsigned long int j = 0; j < n_dims; ++j) mu[0][j] = data[ri][j];
  k_eff = 1;
  vector<double> probs(n_samples, 0);
  double dist = 0;
  double best_dist = 0;
  unsigned long int best_asg = -1;
  unsigned long int cur_asg = -1;
  do {
    for (size_t i = 0; i < n_samples; ++i) {
      best_dist = numeric_limits<double>::max();
      for (size_t c = 0; c < k_eff; ++c) {
        dist = 0;
        for (unsigned long int j = 0; j < n_dims; ++j){
          // dist += distance(data[i][j], mu[c][j], alpha[j], j);
          dist += pow(data[i][j] - mu[c][j], 2);
        }
        if (dist < best_dist) best_dist = dist;
      }
      probs[i] = pow(best_dist, 2);
    }
    boost::random::discrete_distribution<> dist(probs.begin(), probs.end());
    ri = dist(gen);
    for (unsigned long int j = 0; j < n_dims; ++j) mu[k_eff][j] = data[ri][j];
    k_eff += 1;
  } while (k_eff != k);

  for (unsigned long int j = 0; j < n_dims; ++j)
    if (attr_nonnegative[j])
      for (size_t c = 0; c < k_eff; ++c)
        if (mu[c][j] == 0)
          mu[c][j] = 1e-9;
  for (unsigned long int j = 0; j < n_dims; ++j){
    kappa[j] = 1.0;
    kappa_a[j] = 1.0;
    kappa_b[j] = 1.0;
  }
  for (size_t c = 0; c < k_eff; ++c)
    for (unsigned long int j = 0; j < n_dims; ++j){
      mu_a[c][j] = mu[c][j];
      mu_b[c][j] = 1.0;
    }
  for (size_t i = 0; i < n_samples; ++i) {
    best_dist = numeric_limits<double>::max();
    best_asg = -1;
    for (size_t c = 0; c < k; ++c) {
      dist = 0.0;
      for (size_t j = 0; j < n_dims; ++j)
        dist += pow(data[i][j] - mu[c][j], 2);
      if (dist < best_dist) {
        best_dist = dist;
        best_asg = c;
      }
    }
    asg[i] = best_asg;
  }
  for (size_t c = 0; c < k; c++){
    fill(mu[c].begin(), mu[c].end(), 1e-9);
    fill(m2[c].begin(), m2[c].end(), 1e-9);
    fill(m4[c].begin(), m4[c].end(), 1e-9);
  }
  fill(count.begin(), count.end(), 0);
  for (size_t i = 0; i < n_samples; ++i) {
    cur_asg = asg[i];
    count[cur_asg] += 1;
    for (size_t j = 0; j < n_dims; ++j){
      mu[cur_asg][j] += data[i][j];
      m2[cur_asg][j] += pow(data[i][j],2);
      m4[cur_asg][j] += pow(data[i][j],4);
    }
  }
  for (size_t c = 0; c < k; c++)
    for (size_t j = 0; j < n_dims; ++j){
      mu[c][j] = (kappa[j]*mu_a[c][j] + mu[c][j]
                  )/(kappa[j]*mu_b[c][j] + count[c]);
      m2[c][j] = (kappa[j]*pow(mu_a[c][j],2) + m2[c][j]
                  )/(kappa[j]*mu_b[c][j] + count[c]);
      m4[c][j] = (kappa[j]*pow(mu_a[c][j],4) + m4[c][j]
                  )/(kappa[j]*mu_b[c][j] + count[c]);
      v[c][j] = variance(mu[c][j],alpha[j],j);
      w_inv[c][j] = (kappa[j]*mu_b[c][j] + count[c]
        ) / (m4[c][j] - 2*m2[c][j]*kappa[j]*v[c][j] + pow(kappa[j]*v[c][j],2));
    }
}

void AdaHardCluster::fit() {
  bool updated = 0;
  double inertia = 0;
  double nmi = 0;
  double dist = 0;
  double best_dist = 0;
  unsigned long int best_asg = -1;
  unsigned long int cur_asg = -1;
  for (size_t r = 0; r < max_round; r++) {
    updated = 0;
    inertia = 0.0;
    for (size_t i = 0; i < n_samples; ++i) {
      best_dist = numeric_limits<double>::max();
      best_asg = -1;
      for (size_t c = 0; c < k; ++c) {
        dist = distance(data[i], c);
        if (dist < best_dist) {
          best_dist = dist;
          best_asg = c;
        }
      }
      if (asg[i] != best_asg) {
        asg[i] = best_asg;
        updated = 1;
      }
      inertia += best_dist;
    }
    nmi = calc_nmi(label, asg);
    nmis[r] = nmi;
    if (!updated) break;

    for (size_t c = 0; c < k; c++){
      fill(mu[c].begin(), mu[c].end(), 1e-9);
      fill(m2[c].begin(), m2[c].end(), 1e-9);
      fill(m4[c].begin(), m4[c].end(), 1e-9);
    }
    fill(count.begin(), count.end(), 0);
    for (size_t i = 0; i < n_samples; ++i) {
      cur_asg = asg[i];
      count[cur_asg] += 1;
      for (size_t j = 0; j < n_dims; ++j){
        mu[cur_asg][j] += data[i][j];
        m2[cur_asg][j] += pow(data[i][j],2);
        m4[cur_asg][j] += pow(data[i][j],4);
      }
    }
    for (size_t c = 0; c < k; c++)
      for (size_t j = 0; j < n_dims; ++j){
        mu[c][j] = (kappa[j]*mu_a[c][j] + mu[c][j]
                    )/(kappa[j]*mu_b[c][j] + count[c]);
        m2[c][j] = (kappa[j]*pow(mu_a[c][j],2) + m2[c][j]
                    )/(kappa[j]*mu_b[c][j] + count[c]);
        m4[c][j] = (kappa[j]*pow(mu_a[c][j],4) + m4[c][j]
                    )/(kappa[j]*mu_b[c][j] + count[c]);
        v[c][j] = variance(mu[c][j],alpha[j],j);
        w_inv[c][j] = (kappa[j]*mu_b[c][j] + count[c]
          ) / (m4[c][j] - 2*m2[c][j]*kappa[j]*v[c][j] + pow(kappa[j]*v[c][j],2));
      }
    double obj = 0;
    for (size_t j = 0; j < n_dims; ++j){
      CUGMMLite prblm(*this, j);
      copt::Vector<double> lbj;
      lbj.resize(2);
      lbj[0] = lb[j];
      lbj[1] = 1e-9;
      prblm.setLowerBound(lbj);
      copt::Vector<double> ubj;
      ubj.resize(2);
      ubj[0] = ub[j];
      ubj[1] = numeric_limits<double>::max();
      prblm.setUpperBound(ubj);
      copt::Vector<double> xj;
      xj.resize(2);
      xj[0] = alpha[j];
      xj[1] = kappa[j];
      copt::LbfgsbSolver<CUGMMLite> solver;
      solver.minimize(prblm, xj);
      obj += prblm.value(xj);
      if (!isnan(xj[0]))
        alpha[j] = xj[0];
      if (!isnan(xj[1]))
        kappa[j] = xj[1];
    }
    cout << "round=" << r << " nmi=" << nmi << " inertia=" << inertia;
    cout << " obj=" << obj << endl;
  }
  cout << "Alpha=";
  for (unsigned long int j = 0; j < n_dims; ++j)
      cout << alpha[j] << " ";
  cout << endl;
  cout << "Kappa=";
  for (unsigned long int j = 0; j < n_dims; ++j)
      cout << kappa[j] << " ";
  cout << endl;
  for (size_t c = 0; c < k; ++c){
    cout << "mu[" << c << "]=";
    for (unsigned long int j = 0; j < n_dims; ++j)
        cout << mu[c][j] << " ";
    cout << endl;
  }
}


#include <boost/random/normal_distribution.hpp>