/* test_negative_binomial_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/negative_binomial_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::negative_binomial_distribution<>
#define BOOST_RANDOM_ARG1 k
#define BOOST_RANDOM_ARG2 p
#define BOOST_RANDOM_ARG1_DEFAULT 1
#define BOOST_RANDOM_ARG2_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 10
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<int>::max)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<int>::max)()
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<int>::max)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 10

#define BOOST_RANDOM_TEST2_PARAMS (100, 0.5)
#define BOOST_RANDOM_TEST2_MIN 50

#include "test_distribution.ipp"


#ifndef SCROLLGRID3_HPP_I9SAOOSJ
#define SCROLLGRID3_HPP_I9SAOOSJ

#include <math.h>
#include <stdint.h>

#include <Eigen/Core>
#include <Eigen/Dense>

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

#include <pcl_util/point_types.hpp>
#include <geom_cast/geom_cast.hpp>

#include "scrollgrid/mod_wrap.hpp"
#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"

namespace ca
{

/**
 * These are empty functors defining the interface for callbacks
 * to clear cells and fix edges.
 * TODO this could also be achieved with templates.
 */
struct ClearCellsFun {
  virtual void operator()(const Vec3Ix& start,
                          const Vec3Ix& finish) const { }
};

struct FixEdgesFun {
  virtual void operator()(grid_ix_t dim,
                          grid_ix_t trailing,
                          grid_ix_t leading) const { }
};

//typedef boost::function< void (grid_ix_t dim, grid_ix_t trailing, grid_ix_t leading) > FixEdgesFun;
/**
 * "world_xyz" xyz world frame
 * "grid_xyz" xyz grid frame (shifted relative to world_xyz by origin)
 * "grid_ijk" scaled and discretized grid coordinates:
 *     i = floor( (x-origin_x-0.5)/resolution )
 * "local_ijk" grid_ijk shifted by scrolling and limited/wrapped to local extent
 *     li = (i - scroll_offset_i) modulo (dim_i)
 * "mem_ix" index into flat storage from local_ijk.
 *     mem_ix = local_ijk.dot(strides)
 * "hash_ix": bit-packed version of grid_ijk? TODO
 *
 */

// TODO start using vec4 instead for SSE optimization

template<class Scalar>
class ScrollGrid3 {
public:
  // TODO what if used vec4 instead for SSE optimizations?
  typedef Eigen::Matrix<Scalar, 3, 1> Vec3;

  typedef boost::shared_ptr<ScrollGrid3> Ptr;
  typedef boost::shared_ptr<const ScrollGrid3> ConstPtr;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  ScrollGrid3() :
      radius_ijk_(0,0,0),
      box_(),
      origin_(0, 0, 0),
      min_world_corner_ijk_(0, 0, 0),
      dimension_(0, 0, 0),
      num_cells_(0),
      strides_(0, 0, 0),
      scroll_offset_(0, 0, 0),
      last_ijk_(0, 0, 0),
      wrap_ijk_min_(0, 0, 0),
      wrap_ijk_max_(0, 0, 0),
      unwrap_ijk_(0, 0, 0),
      resolution_(0)
  { }

  ScrollGrid3(const Vec3& center,
              const Vec3Ix& dimension,
              Scalar resolution,
              bool x_fastest=false) :
      radius_ijk_(dimension/2),
      box_(center-(radius_ijk_.cast<Scalar>()*resolution),
           center+(radius_ijk_.cast<Scalar>()*resolution)),
      origin_(center-box_.radius()),
      dimension_(dimension),
      num_cells_(dimension.prod()),
      scroll_offset_(0, 0, 0),
      last_ijk_(scroll_offset_ + dimension_),
      wrap_ijk_min_(0, 0, 0),
      wrap_ijk_max_(0, 0, 0),
      unwrap_ijk_(0, 0, 0),
      resolution_(resolution)
  {
    // calculate coordinates of min corner in ijk
    Scalar m = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    Vec3 m3(m, m, m);
    min_world_corner_ijk_ = this->world_to_grid(m3);

    if (x_fastest) {
      strides_ = Vec3Ix(1, dimension[0], dimension.head<2>().prod());
    } else {
      strides_ = Vec3Ix(dimension.tail<2>().prod(), dimension[2], 1);
    }

    this->update_wrap_ijk();
  }

  virtual ~ScrollGrid3() { }

  ScrollGrid3(const ScrollGrid3& other) :
      box_(other.box_),
      origin_(other.origin_),
      min_world_corner_ijk_(other.min_world_corner_ijk_),
      dimension_(other.dimension_),
      num_cells_(other.num_cells_),
      strides_(other.strides_),
      scroll_offset_(other.scroll_offset_),
      last_ijk_(other.last_ijk_),
      wrap_ijk_min_(other.wrap_ijk_min_),
      wrap_ijk_max_(other.wrap_ijk_max_),
      unwrap_ijk_(other.unwrap_ijk_),
      resolution_(other.resolution_)
  {
  }

  ScrollGrid3& operator=(const ScrollGrid3& other) {
    if (this==&other) { return *this; }
    box_ = other.box_;
    origin_ = other.origin_;
    min_world_corner_ijk_ = other.min_world_corner_ijk_;
    dimension_ = other.dimension_;
    num_cells_ = other.num_cells_;
    strides_ = other.strides_;
    scroll_offset_ = other.scroll_offset_;
    last_ijk_ = other.last_ijk_;
    wrap_ijk_min_ = other.wrap_ijk_min_;
    wrap_ijk_max_ = other.wrap_ijk_max_;
    unwrap_ijk_ = other.unwrap_ijk_;
    resolution_ = other.resolution_;
    return *this;
  }

public:

  void reset(const Vec3& center,
             const Vec3Ix& dimension,
             Scalar resolution,
             bool x_fastest=false) {

    radius_ijk_ = dimension/2;
    box_.set_center(center);
    box_.set_radius(radius_ijk_.cast<Scalar>() * resolution);
    origin_ = center - box_.radius();

    dimension_ = dimension;
    num_cells_ = dimension.prod();
    if (x_fastest) {
      strides_ = Vec3Ix(1, dimension[0], dimension.head<2>().prod());
    } else {
      strides_ = Vec3Ix(dimension.tail<2>().prod(), dimension[2], 1);
    }
    scroll_offset_.setZero();
    last_ijk_ = scroll_offset_ + dimension_;

    this->update_wrap_ijk();

    resolution_ = resolution;

    Vec3 m;
    m[0] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    m[1] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    m[2] = -static_cast<Scalar>(std::numeric_limits<uint16_t>::max()/2)*resolution_;
    min_world_corner_ijk_ = this->world_to_grid(m);

  }

  /**
   * Is inside 3D box containing grid?
   * @param pt point in same frame as center (probably world_view)
   */
  bool is_inside_box(const Vec3& pt) const {
    return box_.contains(pt);
  }

  template<class PointT>
  bool is_inside_box(const PointT& pt) const {
    return box_.contains(ca::point_cast<Vec3>(pt));
  }

  /**
   * is i, j, k inside the grid limits?
   */
  bool is_inside_grid(const Vec3Ix& grid_ix) const {
    return ((grid_ix.array() >= scroll_offset_.array()).all() &&
            (grid_ix.array() < (scroll_offset_+dimension_).array()).all());
  }

  bool is_inside_grid(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->is_inside_grid(Vec3Ix(i, j, k));
  }

  /**
   * scroll grid.
   * updates bounding box and offset_cells.
   * @param offset_cells. how much to scroll. offset_cells is a signed integral.
   *
   */
  void just_scroll(const Vec3Ix& offset_cells) {
    Vec3Ix new_offset = scroll_offset_ + offset_cells;
    box_.translate((offset_cells.cast<Scalar>()*resolution_));
    scroll_offset_ = new_offset;
    last_ijk_ = scroll_offset_ + dimension_;

    this->update_wrap_ijk();
  }

/**
   * functionally same as just_scroll.
   * special case of scroll_and_clear_and_fix.
   */
  void scroll(const Vec3Ix& offset_cells) {
    ClearCellsFun nullclear;
    FixEdgesFun nullfix;
    this->scroll_and_clear_and_fix(offset_cells, nullclear, nullfix);
  }

/**
   * scroll grid by offset_cells and call clear_cells_fun on outgoing/incoming
   * cells.
   * special case of scroll_and_clear_and_fix.
   */

  void scroll_and_clear(const Vec3Ix& offset_cells,
                        const ClearCellsFun& clear_cells_fun) {
    FixEdgesFun nullfix;
    this->scroll_and_clear_and_fix(offset_cells, clear_cells_fun, nullfix);
  }

/**
   * scroll grid by offset_cells, call fix_edges_fun on outgoing/incoming
   * edges, and clear_cells_fun on outgoing/incoming cells.
   */
  void scroll_and_clear_and_fix(const Vec3Ix& offset_cells,
                                const ClearCellsFun& clear_cells_fun,
                                const FixEdgesFun& fix_edges_fun) {

    Vec3Ix new_offset = scroll_offset_ + offset_cells;

    // check if there is overlap between current box and box after scroll.
    // if there is not, then the whole box must be wiped out.
    if (( abs(offset_cells[0]) >= dimension_[0] ) ||
        ( abs(offset_cells[1]) >= dimension_[1] ) ||
        ( abs(offset_cells[2]) >= dimension_[2] ) ) {

      clear_cells_fun(scroll_offset_, scroll_offset_+dimension_);

      // not sure if *all* edges must be fixed or none of them should.
      // according to current logic none of them should.
      // TODO caveat: there is an -1 offset in all the edges calculations.
      // also note: earlier logic completely ignored this case.

    } else {
      if (offset_cells[0] > 0) {
        Vec3Ix finish(new_offset[0],
                      scroll_offset_[1] + dimension_[1],
                      scroll_offset_[2] + dimension_[2]);
        clear_cells_fun(scroll_offset_, finish);
        fix_edges_fun(0, finish[0], scroll_offset_[0]+dimension_[0]-1);
      } else if (offset_cells[0] < 0) {
        Vec3Ix start(scroll_offset_[0]+dimension_[0]+offset_cells[0],
                     scroll_offset_[1],
                     scroll_offset_[2]);
        Vec3Ix finish = scroll_offset_ + dimension_;
        clear_cells_fun(start, finish);
        fix_edges_fun(0, start[0]-1, scroll_offset_[0]);
      }

      if (offset_cells[1] > 0) {
        Vec3Ix finish(scroll_offset_[0] + dimension_[0],
                      new_offset[1],
                      scroll_offset_[2] + dimension_[2]);
        clear_cells_fun(scroll_offset_, finish);
        fix_edges_fun(1, finish[1], scroll_offset_[1]+dimension_[1]-1);
      } else if(offset_cells[1] < 0) {
        Vec3Ix start(scroll_offset_[0],
                     scroll_offset_[1]+dimension_[1]+offset_cells[1],
                     scroll_offset_[2]);
        Vec3Ix finish = scroll_offset_ + dimension_;
        clear_cells_fun(start, finish);
        fix_edges_fun(1, start[1]-1, scroll_offset_[1]);
      }

      if (offset_cells[2] > 0) {
        Vec3Ix finish(scroll_offset_[0] + dimension_[0],
                      scroll_offset_[1] + dimension_[1],
                      new_offset[2]);
        clear_cells_fun(scroll_offset_, finish);
        fix_edges_fun(2, finish[2], scroll_offset_[2]+dimension_[2]-1);
      } else if(offset_cells[2] < 0) {
        Vec3Ix start(scroll_offset_[0],
                     scroll_offset_[1],
                     scroll_offset_[2]+dimension_[2]+offset_cells[2]);
        Vec3Ix finish = scroll_offset_ + dimension_;
        clear_cells_fun(start, finish);
        fix_edges_fun(2, start[2]-1, scroll_offset_[2]);
      }
    }

    box_.translate((offset_cells.cast<Scalar>()*resolution_));
    scroll_offset_ = new_offset;
    last_ijk_ = scroll_offset_ + dimension_;
    this->update_wrap_ijk();

  }

  /**
   * get boxes to clear if scrolling by offset_cells.
   * Call this *before* scroll().
   * @param clear_i_min min corner of obsolete region in grid
   * @param clear_i_max max corner of obsolete region in grid
   * same for j and k
   * Note that boxes may overlap.
   *
   * Note: this may be deprecated in favor of the clearfun callbacks
   * in scroll.
   */
  void get_clear_boxes(const Vec3Ix& offset_cells,
                       Vec3Ix& clear_i_min, Vec3Ix& clear_i_max,
                       Vec3Ix& clear_j_min, Vec3Ix& clear_j_max,
                       Vec3Ix& clear_k_min, Vec3Ix& clear_k_max) {

    clear_i_min.setZero();
    clear_j_min.setZero();
    clear_k_min.setZero();
    clear_i_max.setZero();
    clear_j_max.setZero();
    clear_k_max.setZero();

    // first check if there is overlap between current and box after scroll.
    // if there is not, then the whole box must be wiped out.
    if (( abs(offset_cells[0]) >= dimension_[0] ) ||
        ( abs(offset_cells[1]) >= dimension_[1] ) ||
        ( abs(offset_cells[2]) >= dimension_[2] ) ) {
      clear_i_min = scroll_offset_;
      clear_i_max = scroll_offset_ + dimension_;
      return;
    }

    Vec3Ix new_offset = scroll_offset_ + offset_cells;

    // X axis
    if (offset_cells[0] > 0) {
      clear_i_min = scroll_offset_;
      clear_i_max = Vec3Ix(new_offset[0],
                           scroll_offset_[1]+dimension_[1],
                           scroll_offset_[2]+dimension_[2]);
    } else if (offset_cells[0] < 0) {
      clear_i_min = Vec3Ix(scroll_offset_[0]+dimension_[0]+offset_cells[0],
                           scroll_offset_[1],
                           scroll_offset_[2]);
      clear_i_max = scroll_offset_ + dimension_;
    }

    // Y axis
    if (offset_cells[1] > 0) {
      clear_j_min = scroll_offset_;
      clear_j_max = Vec3Ix(scroll_offset_[0]+dimension_[0],
                           new_offset[1],
                           scroll_offset_[2]+dimension_[2]);
    } else if (offset_cells[1] < 0) {
      clear_j_min = Vec3Ix(scroll_offset_[0],
                           scroll_offset_[1]+dimension_[1]+offset_cells[1],
                           scroll_offset_[2]);
      clear_j_max = scroll_offset_ + dimension_;
    }

    // Z axis
    if (offset_cells[2] > 0) {
      clear_k_min = scroll_offset_;
      clear_k_max = Vec3Ix(scroll_offset_[0]+dimension_[0],
                           scroll_offset_[1]+dimension_[1],
                           new_offset[2]);
    } else if(offset_cells[2] < 0) {
      clear_k_min = Vec3Ix(scroll_offset_[0],
                           scroll_offset_[1],
                           scroll_offset_[2]+dimension_[2]+offset_cells[2]);
      clear_k_max = scroll_offset_ + dimension_;
    }

  }

  /**
   * Given position in world coordinates, return grid coordinates.
   * (grid coordinates are not wrapped to be inside grid!)
   * Note: does not check if point is inside grid.
   */
  Vec3Ix world_to_grid(const Vec3& xyz) const {
    Vec3 tmp = ((xyz - origin_).array() - 0.5*resolution_)/resolution_;
    return Vec3Ix(round(tmp.x()), round(tmp.y()), round(tmp.z()));
  }

  Vec3Ix world_to_grid(Scalar x, Scalar y, Scalar z) const {
    return this->world_to_grid(Vec3(x, y, z));
  }

  /**
   * Like world to grid but xyz are offset by scroll grid center.
   * DON'T USE OR YOU WILL SCREW UP.
   */
  Vec3Ix offset_world_to_grid(const Vec3& xyz) const {
    Vec3 tmp = ((xyz + box_.center() - origin_).array() - 0.5*resolution_)/resolution_;
    return Vec3Ix(round(tmp.x()), round(tmp.y()), round(tmp.z()));
  }

  Vec3Ix offset_world_to_grid(Scalar x, Scalar y, Scalar z) const {
    return this->offset_world_to_grid(Vec3(x, y, z));
  }

  Vec3 grid_to_world(const Vec3Ix& grid_ix) const {
    Vec3 w((grid_ix.cast<Scalar>()*resolution_ + origin_).array() + 0.5*resolution_);
    return w;
  }

  Vec3 grid_to_world(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->grid_to_world(Vec3Ix(i, j, k));
  }

  /**
   * Translate grid indices to an address in linear memory.
   * Does not check if grid_ix is inside current grid box.
   * Assumes C-order, x the slowest and z the fastest.
   */
  mem_ix_t grid_to_mem_slow(const Vec3Ix& grid_ix) const {
    Vec3Ix grid_ix2(ca::mod_wrap(grid_ix[0], dimension_[0]),
                    ca::mod_wrap(grid_ix[1], dimension_[1]),
                    ca::mod_wrap(grid_ix[2], dimension_[2]));
    return strides_.dot(grid_ix2);
  }

  /**
   * Faster than grid_to_mem_slow, as it avoids modulo.
   * But it only works if the grid_ix are inside the bounding box.
   * Hopefully branch prediction kicks in when using this in a loop.
   */
  mem_ix_t grid_to_mem(const Vec3Ix& grid_ix) const {
    ROS_ASSERT( this->is_inside_grid(grid_ix) );

    Vec3Ix grid_ix2(grid_ix);

    if (grid_ix2[0] >= wrap_ijk_max_[0]) { grid_ix2[0] -= wrap_ijk_max_[0]; } else { grid_ix2[0] -= wrap_ijk_min_[0]; }
    if (grid_ix2[1] >= wrap_ijk_max_[1]) { grid_ix2[1] -= wrap_ijk_max_[1]; } else { grid_ix2[1] -= wrap_ijk_min_[1]; }
    if (grid_ix2[2] >= wrap_ijk_max_[2]) { grid_ix2[2] -= wrap_ijk_max_[2]; } else { grid_ix2[2] -= wrap_ijk_min_[2]; }

    mem_ix_t mem_ix2 = strides_.dot(grid_ix2);

    return mem_ix2;
  }

  mem_ix_t grid_to_mem_slow(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return this->grid_to_mem_slow(Vec3Ix(i, j, k));
  }

  mem_ix_t grid_to_mem(grid_ix_t i, grid_ix_t j, grid_ix_t k) const {
    return grid_to_mem( Vec3Ix(i, j, k) );
  }

  /**
   * TODO should we do this in the sparse_array3 module?
   */
  uint64_t grid_to_hash(const Vec3Ix& grid_ix) const {
    // grid2 should be all positive
    Vec3Ix grid2(grid_ix - min_world_corner_ijk_);

    uint64_t hi = static_cast<uint64_t>(grid2[0]);
    uint64_t hj = static_cast<uint64_t>(grid2[1]);
    uint64_t hk = static_cast<uint64_t>(grid2[2]);
    uint64_t h = (hi << 48) | (hj << 32) | (hk << 16);
    return h;
  }

  Vec3Ix hash_to_grid(uint64_t hix) const {
    uint64_t hi = (hix & 0xffff000000000000) >> 48;
    uint64_t hj = (hix & 0x0000ffff00000000) >> 32;
    uint64_t hk = (hix & 0x00000000ffff0000) >> 16;
    Vec3Ix grid_ix(hi, hj, hk);
    grid_ix += min_world_corner_ijk_;
    return grid_ix;
  }

  /**
   * Note that no bound check is performed!
   */
  mem_ix_t world_to_mem(const Vec3& xyz) const {
    Vec3Ix gix(this->world_to_grid(xyz));
    return this->grid_to_mem(gix);
  }

  Vec3Ix mem_to_grid(grid_ix_t mem_ix) const {
    // TODO does this work for x-fastest strides?
    grid_ix_t i = mem_ix/strides_[0];
    mem_ix -= i*strides_[0];
    grid_ix_t j = mem_ix/strides_[1];
    mem_ix -= j*strides_[1];
    grid_ix_t k = mem_ix;

    if (i < unwrap_ijk_[0]) { i += wrap_ijk_max_[0]; } else { i += wrap_ijk_min_[0]; }
    if (j < unwrap_ijk_[1]) { j += wrap_ijk_max_[1]; } else { j += wrap_ijk_min_[1]; }
    if (k < unwrap_ijk_[2]) { k += wrap_ijk_max_[2]; } else { k += wrap_ijk_min_[2]; }

    return (Vec3Ix(i, j, k));

  }

 public:
  grid_ix_t dim_i() const { return dimension_[0]; }
  grid_ix_t dim_j() const { return dimension_[1]; }
  grid_ix_t dim_k() const { return dimension_[2]; }
  grid_ix_t first_i() const { return scroll_offset_[0]; }
  grid_ix_t first_j() const { return scroll_offset_[1]; }
  grid_ix_t first_k() const { return scroll_offset_[2]; }
  grid_ix_t last_i() const { return last_ijk_[0]; }
  grid_ix_t last_j() const { return last_ijk_[1]; }
  grid_ix_t last_k() const { return last_ijk_[2]; }
  const Vec3Ix& scroll_offset() const { return scroll_offset_; }
  const Vec3Ix& unwrap_ijk() const { return unwrap_ijk_; }
//   const Vec3Ix& unwrap_ijk_() const { return unwrap_ijk_; }
  const Vec3Ix& radius_ijk() const { return radius_ijk_; }
  const Vec3Ix& dimension() const { return dimension_; }
  const Vec3& radius() const { return box_.radius(); }
  const Vec3& origin() const { return origin_; }
  Vec3 min_pt() const { return box_.min_pt(); }
  Vec3 max_pt() const { return box_.max_pt(); }
  const Vec3& center() const { return box_.center(); }
  Scalar resolution() const { return resolution_; }
  const ca::scrollgrid::Box<Scalar, 3>& box() const { return box_; }

  grid_ix_t num_cells() const { return num_cells_; }

 private:

  /**
   * (Re)calculate indices where wrapping occurs for faster lookups.
   * This basically avoids modulo operations.
   * Must be called each time scrollgrid moves.
   */
  void update_wrap_ijk() {
    wrap_ijk_min_[0] = floor(static_cast<float>(scroll_offset_[0])/dimension_[0])*dimension_[0];
    wrap_ijk_min_[1] = floor(static_cast<float>(scroll_offset_[1])/dimension_[1])*dimension_[1];
    wrap_ijk_min_[2] = floor(static_cast<float>(scroll_offset_[2])/dimension_[2])*dimension_[2];

    wrap_ijk_max_[0] = floor(static_cast<float>(scroll_offset_[0]+dimension_[0])/dimension_[0])*dimension_[0];
    wrap_ijk_max_[1] = floor(static_cast<float>(scroll_offset_[1]+dimension_[1])/dimension_[1])*dimension_[1];
    wrap_ijk_max_[2] = floor(static_cast<float>(scroll_offset_[2]+dimension_[2])/dimension_[2])*dimension_[2];

    unwrap_ijk_[0] = ca::mod_wrap(scroll_offset_[0], dimension_[0]);
    unwrap_ijk_[1] = ca::mod_wrap(scroll_offset_[1], dimension_[1]);
    unwrap_ijk_[2] = ca::mod_wrap(scroll_offset_[2], dimension_[2]);

  }

 private:

  // discrete radius, used when calculating box dimensions
  Vec3Ix radius_ijk_;

  // 3d box enclosing grid. In whatever coordinates were given (probably
  // world_view)
  ca::scrollgrid::Box<Scalar, 3> box_;

  // xyz position of grid origin in world_xyz frame.
  // does not move when scrolling.
  // initialized as (center - box.radius).
  Vec3 origin_;

  // minimum world corner in ijk. used for hash
  Vec3Ix min_world_corner_ijk_;

  // number of grid cells along each axis
  Vec3Ix dimension_;

  // number of cells
  grid_ix_t num_cells_;

  // grid strides to translate from linear to 3D layout.
  // C-ordering, ie x slowest, z fastest.
  Vec3Ix strides_;

  // to keep track of scrolling along z.
  Vec3Ix scroll_offset_;

  // redundant but actually seems to have a performance benefit
  // should always be dimension + offset
  Vec3Ix last_ijk_;

  // for grid_to_mem. the points where the grid crosses modulo boundaries. // nearly boundary pt
  Vec3Ix wrap_ijk_min_;
  Vec3Ix wrap_ijk_max_;

  // delimits when extra offset needs to be added when unwrapping  offset % dimension 余数
  Vec3Ix unwrap_ijk_;

  // size of grid cells
  Scalar resolution_;

};

typedef ScrollGrid3<float> ScrollGrid3f;
typedef ScrollGrid3<double> ScrollGrid3d;

} /* ca */

#endif /* end of include guard: SCROLLGRID3_HPP_I9SAOOSJ */


//
// GibbsSamplerFromGMOM.hpp
//
// Copyright (c) 2017 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#ifndef GIBBSGMOM
#define GIBBSGMOM

#include<stdlib.h>
#include<math.h>
#include<cmath>
#include<iostream>
#include<vector>
#include<numeric>
#include<memory>
#include<random>
#include<iomanip>
#include<fstream>
#include<limits>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include"CsvFileParser.hpp"
#include"utils.hpp"

namespace bmpi = boost::mpi;

double calculateLogBetaFunction(std::vector<double> alpha, int i=-1);

double calculateDirichletLogPDF(std::vector<double> x, std::vector<double> alpha, int i=-1);

unsigned int calculateFactorial(unsigned int x);

class GibbsSamplerFromGMOM{
protected:
    const std::vector<std::vector<double> > &_logO, &_U;
    const unsigned int _N, _M, _G;
    const unsigned int _iterationNumber, _burnIn, _samplingInterval;
    const double _k, _theta, _A;
    std::vector<std::vector<unsigned int> > _V;
    std::vector<std::vector<double> > _sumOfVSampled;
    std::vector<std::vector<double> > _P, _sumOfPSampled;
    std::vector<std::vector<double> > _gamma;
    unsigned int _samplingCount;
    std::vector<double> _logLikelihood;
    bmpi::communicator _world;
public:
    GibbsSamplerFromGMOM(const CsvFileParser<double> &orthologFile, const CsvFileParser<double> &microbeFile, double A, double k, double theta, unsigned int iterationNumber, unsigned int burnIn, unsigned int samplingInterval, bmpi::communicator &world);
    virtual ~GibbsSamplerFromGMOM();
    virtual void initializeParameters();
    virtual void updateGamma(unsigned int j, unsigned int k, int deltaVjk);
    virtual double calculateDirichletLogPDF(int k=-1);
    virtual void sampleV();
    virtual void sampleP();
    virtual void calculateLogLikelihood();
    virtual void writeParameters(std::string PFilename, std::string VFilename)const;
    virtual void writeLogLikelihood(std::string logLikelihoodFilename)const;
    virtual void storeSamples();
    virtual void runIteraions();
};

#endif


#include "neal3_algorithm.h"

#include <Eigen/Dense>
#include <memory>

#include "hierarchy_id.pb.h"
#include "mixing_id.pb.h"
#include "src/hierarchies/base_hierarchy.h"
#include "src/mixings/marginal_mixing.h"

Eigen::VectorXd Neal3Algorithm::get_cluster_lpdf(
    const unsigned int data_idx) const {
  unsigned int n_data = data.rows();
  unsigned int n_clust = unique_values.size();
  Eigen::VectorXd loglpdf(n_clust + 1);
  for (size_t j = 0; j < n_clust; j++) {
    // Probability of being assigned to an already existing cluster
    loglpdf(j) = unique_values[j]->conditional_pred_lpdf(
        data.row(data_idx), hier_covariates.row(data_idx));
  }
  // Probability of being assigned to a newly created cluster
  loglpdf(n_clust) = unique_values[0]->prior_pred_lpdf(
      data.row(data_idx), hier_covariates.row(data_idx));
  return loglpdf;
}

void Neal3Algorithm::print_startup_message() const {
  std::string msg = "Running Neal3 algorithm with " +
                    bayesmix::HierarchyId_Name(unique_values[0]->get_id()) +
                    " hierarchies, " +
                    bayesmix::MixingId_Name(marg_mixing->get_id()) +
                    " mixing...";
  std::cout << msg << std::endl;
}


#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/numeric/conversion/cast.hpp>

//typedef boost::multiprecision::cpp_dec_float_50 xmc_float;
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<64> > xmc_float;
typedef boost::multiprecision::cpp_int xmc_int;
typedef boost::multiprecision::uint128_t xmc_uint_128;

const xmc_float XMC_UINT = xmc_float(1000000000000.0);
const xmc_uint_128 XMC_INT_MAX = xmc_uint_128((uint64_t)10000000000000000000ull);

inline double xmc_int_to_double(xmc_int amount) {
        xmc_uint_128 amount_128 = amount.convert_to<xmc_uint_128>();
        //std::cout<<"uint128_t amount:" << amount_128 << std::endl;

	if(amount_128 < XMC_INT_MAX)
	{
		uint64_t int_amount = amount_128.convert_to<uint64_t>();
		//std::cout<< "amount < XMC_DEFAULT_DECIMAL :" << int_amount << std::endl;
		double ret = int_amount / 1000000000000.0;
		//std::cout<< "****** return value:" << ret << std::endl;
		return ret;
	}
	//std::setprecision(std::numeric_limits<xmc_float>::max_digits10);
	//std::cout<<"# XMC int to double ==> xmc_int:" << amount << std::endl;
	//xmc_float amount_float = amount_128.convert_to<xmc_float>();
	xmc_float amount_float = xmc_float(amount_128);
	//std::cout<<"# XMC int to double ==> xmc_float:" << amount_float << std::endl;
	xmc_float amount_xmc = amount_float / XMC_UINT;
	//std::cout<<"#XMC int to double ==> amount_xmc:" << amount_xmc << std::endl;
	double ret = amount_xmc.convert_to<double>();
	//std::cout<<"#XMC int to double ==> result:" << ret << std::endl;
	return ret;
}


#ifndef Integration_ModifiedArrheniusIntegralBase_hpp
#define Integration_ModifiedArrheniusIntegralBase_hpp

/** @file ModifiedArrheniusIntegralBase.hpp
  * @brief
  * @author C.D. Clark III
  * @date 06/27/17
  */

#include <boost/math/tools/roots.hpp>
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::eps_tolerance;
#include "ArrheniusIntegralBase.hpp"

namespace libArrhenius {

/** @class ModifiedArrheniusIntegralBase
  * @brief Base class for common modified Arrhenius integral data.
  * @author C.D. Clark III
  */
template<typename Real>
class ModifiedArrheniusIntegralBase : public ArrheniusIntegralBase<Real>
{
  protected:
    Real n;

  public:

    template<typename T>
    void setExponent( T n_ ) { n = n_; }
    Real getExponent( ) { return n; }

    Real getCriticalTemperature() const {
      Real Tcrit = ArrheniusIntegralBase<Real>::getCriticalTemperature();
      if( n == 0 )
        return Tcrit;

      // if n != 0, then we have a transcendental equation we need to solve.
      //
      // ln T^n - Ea/R/T + ln A = 0  (solve for T)
      //
      // use the n = 0 case as a first gues. this will be too high
      Real factor = 1.1;
      eps_tolerance<Real> tol( std::numeric_limits<Real>::digits - 3 );
      boost::uintmax_t maxit = 100;
      auto Tcrit_range =  bracket_and_solve_root(
               [&](Real T){
               return Constants::MKS::R*T*log(this->A*pow(T,this->n)) - this->Ea; },
               Tcrit, factor, true, tol, maxit );
      Tcrit = Tcrit_range.first;
      return Tcrit;
    }

    Real rate( Real T ) const { return pow(T,n)*this->A*exp( -this->Ea / Constants::MKS::R /T ); }

  protected:
};

}

#endif // include protector


/*
Copyright (C) 2016 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 "toplevelfixture.hpp"
#include "utilities.hpp"
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include <qle/termstructures/interpolatedyoycapfloortermpricesurface.hpp>

#include <ql/indexes/inflation/euhicp.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/matrix.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/inflation/piecewisezeroinflationcurve.hpp>
#include <ql/termstructures/volatility/equityfx/blackvariancesurface.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>

using namespace QuantExt;
using namespace QuantLib;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CapFloordTermPriceSurfaceTest)

BOOST_AUTO_TEST_CASE(testInterpolatedYoyCapFloorTermPriceSurface) {

    BOOST_TEST_MESSAGE("Testing InterpolatedYoyCapFloorTermPriceSurface");

    Date asof_ = Date(18, July, 2016);
    Settings::instance().evaluationDate() = asof_;

    Handle<YieldTermStructure> nominalTs =
        Handle<YieldTermStructure>(boost::make_shared<FlatForward>(0, TARGET(), 0.005, Actual365Fixed()));

    std::vector<Rate> capStrikes;
    capStrikes.push_back(0.01);
    capStrikes.push_back(0.02);
    capStrikes.push_back(0.03);
    capStrikes.push_back(0.04);
    capStrikes.push_back(0.05);

    std::vector<Rate> floorStrikes;
    floorStrikes.push_back(-0.02);
    floorStrikes.push_back(-0.01);
    floorStrikes.push_back(0.0);
    floorStrikes.push_back(0.01);
    floorStrikes.push_back(0.02);

    std::vector<Period> maturities;
    maturities.push_back(Period(2, Years));
    maturities.push_back(Period(5, Years));
    maturities.push_back(Period(7, Years));
    maturities.push_back(Period(10, Years));
    maturities.push_back(Period(15, Years));
    maturities.push_back(Period(20, Years));

    Matrix capPrice(capStrikes.size(), maturities.size(), Null<Real>()),
        floorPrice(floorStrikes.size(), maturities.size(), Null<Real>());

    capPrice[0][0] = 0.00874;
    capPrice[1][0] = 0.00146;
    capPrice[2][0] = 0.00019;
    capPrice[3][0] = 0.00003;
    capPrice[4][0] = 0.00001;
    capPrice[0][1] = 0.02946;
    capPrice[1][1] = 0.00793;
    capPrice[2][1] = 0.00214;
    capPrice[3][1] = 0.00074;
    capPrice[4][1] = 0.00032;
    capPrice[0][2] = 0.04626;
    capPrice[1][2] = 0.01448;
    capPrice[2][2] = 0.00481;
    capPrice[3][2] = 0.00206;
    capPrice[4][2] = 0.00107;
    capPrice[0][3] = 0.07622;
    capPrice[1][3] = 0.02778;
    capPrice[2][3] = 0.01106;
    capPrice[3][3] = 0.00565;
    capPrice[4][3] = 0.00343;
    capPrice[0][4] = 0.13218;
    capPrice[1][4] = 0.05476;
    capPrice[2][4] = 0.0245;
    capPrice[3][4] = 0.01393;
    capPrice[4][4] = 0.00927;
    capPrice[0][5] = 0.18889;
    capPrice[1][5] = 0.08297;
    capPrice[2][5] = 0.03909;
    capPrice[3][5] = 0.02369;
    capPrice[4][5] = 0.01674;
    floorPrice[0][0] = 0.000000001;
    floorPrice[1][0] = 0.00005;
    floorPrice[2][0] = 0.00057;
    floorPrice[3][0] = 0.00415;
    floorPrice[4][0] = 0.01695;
    floorPrice[0][1] = 0.00005;
    floorPrice[1][1] = 0.00071;
    floorPrice[2][1] = 0.00259;
    floorPrice[3][1] = 0.01135;
    floorPrice[4][1] = 0.03983;
    floorPrice[0][2] = 0.00035;
    floorPrice[1][2] = 0.00131;
    floorPrice[2][2] = 0.00482;
    floorPrice[3][2] = 0.0169;
    floorPrice[4][2] = 0.05463;
    floorPrice[0][3] = 0.0014;
    floorPrice[1][3] = 0.0036;
    floorPrice[2][3] = 0.00943;
    floorPrice[3][3] = 0.02584;
    floorPrice[4][3] = 0.07515;
    floorPrice[0][4] = 0.00481;
    floorPrice[1][4] = 0.00904;
    floorPrice[2][4] = 0.01814;
    floorPrice[3][4] = 0.04028;
    floorPrice[4][4] = 0.10449;
    floorPrice[0][5] = 0.00832;
    floorPrice[1][5] = 0.01433;
    floorPrice[2][5] = 0.02612;
    floorPrice[3][5] = 0.05269;
    floorPrice[4][5] = 0.12839;

    // build a
    std::vector<Date> datesZCII;
    datesZCII.push_back(asof_ + 1 * Years);
    std::vector<Rate> ratesZCII;
    ratesZCII.push_back(1.1625);

    // build EUHICPXT fixing history
    Schedule fixingDatesEUHICPXT =
        MakeSchedule().from(Date(1, May, 2015)).to(Date(1, July, 2016)).withTenor(1 * Months);
    std::vector<Real> fixingRatesEUHICPXT(15, 100);

    Handle<ZeroInflationIndex> hEUHICPXT;
    boost::shared_ptr<EUHICPXT> ii = boost::shared_ptr<EUHICPXT>(new EUHICPXT(false));
    boost::shared_ptr<ZeroInflationTermStructure> cpiTS;
    for (Size i = 0; i < fixingDatesEUHICPXT.size(); i++) {
        ii->addFixing(fixingDatesEUHICPXT[i], fixingRatesEUHICPXT[i], true);
    };
    // now build the helpers ...
    std::vector<boost::shared_ptr<BootstrapHelper<ZeroInflationTermStructure> > > instruments;
    for (Size i = 0; i < datesZCII.size(); i++) {
        Handle<Quote> quote(boost::shared_ptr<Quote>(new SimpleQuote(ratesZCII[i] / 100.0)));
        boost::shared_ptr<BootstrapHelper<ZeroInflationTermStructure> > anInstrument(new ZeroCouponInflationSwapHelper(
            quote, Period(3, Months), datesZCII[i], TARGET(), ModifiedFollowing, Actual365Fixed(), ii, nominalTs));
        instruments.push_back(anInstrument);
    };

    Rate baseZeroRate = ratesZCII[0] / 100.0;
    boost::shared_ptr<PiecewiseZeroInflationCurve<Linear>> pCPIts(new PiecewiseZeroInflationCurve<Linear>(
        asof_, TARGET(), Actual365Fixed(), Period(3, Months), Monthly, false, baseZeroRate, instruments));
    pCPIts->recalculate();
    cpiTS = boost::dynamic_pointer_cast<ZeroInflationTermStructure>(pCPIts);

    boost::shared_ptr<EUHICPXT> zii(new EUHICPXT(false, Handle<ZeroInflationTermStructure>(pCPIts)));
    boost::shared_ptr<ZeroInflationIndex> zeroIndex = boost::dynamic_pointer_cast<ZeroInflationIndex>(zii);

    boost::shared_ptr<YoYInflationIndex> yoyIndex;

    yoyIndex =
        boost::make_shared<QuantExt::YoYInflationIndexWrapper>(zeroIndex, true, Handle<YoYInflationTermStructure>());

    QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> ys(
        0, Period(3, Months), yoyIndex, 1, nominalTs, Actual365Fixed(), TARGET(), Following, capStrikes, floorStrikes,
        maturities, capPrice, floorPrice);

    boost::shared_ptr<QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> > yoySurface =
        boost::make_shared<QuantExt::InterpolatedYoYCapFloorTermPriceSurface<Bilinear, Linear> >(ys);

    // check the cap and floor prices from the surface
    Real tol = 1.0E-8;
    for (Size i = 0; i < maturities.size(); i++) {
        Date m = yoySurface->yoyOptionDateFromTenor(maturities[i]);
        for (Size j = 0; j < capStrikes.size(); j++) {
            BOOST_CHECK_CLOSE(yoySurface->capPrice(m, capStrikes[j]), capPrice[j][i], tol);
        }
        for (Size j = 0; j < floorStrikes.size(); j++) {
            BOOST_CHECK_CLOSE(yoySurface->floorPrice(m, floorStrikes[j]), floorPrice[j][i], tol);
        }
    }
} // testInterpolatedYoyCapFloorTermPriceSurface

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (C) 2017 IBM Corp.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/**********************************************************
 * test_TDMatrix: a program for testing the TDMatrix module
 * prints out PASS/FAIL information. If a certain bit fails,
 * the bit index is returned in the message
***********************************************************/
//#include <mpfr.h>
#include <NTL/lzz_p.h>

NTL_CLIENT
#include "../utils/argmap.h"
#include "../TDMatrix.h"

//#define VERBOSE //check extra information?

int main(int argc, char *argv[])
{
#ifdef DEBUG
    NTL::SetSeed((unsigned char*)"test_sampleG",12);
#endif
    ArgMapping amap;
    long k = 2;
    amap.arg("k", k, "number of factors (~ log q)");
    long n = 2;
    amap.arg("n", n, "the small dimension"); // no default info
    long e=2;
    amap.arg("e", e, "the power of each factor");
    long m=0;
    amap.arg("m", m, "the dimension m", "0: set as n*(2+k*e)");

    amap.parse(argc, argv); // parses and overrides initail values
    // for each parameter, one per line,

    if (m < n*(2+k*e)) m = n*(2+k*e);

    TDMatrixParams params(n,k,e,m);
    //    Vec<long>& factors = params.factors;

    cout << "n=" << n << ", m=" << m << ", k="
         << k     << ", e=" << e
	 << ", sigmaX=" << params.sigmaX << ", q=" << params.getQ() << endl;

    long mBar = params.mBar;
    TDMatrix aMat(params);
    CRTmatrix A;
    aMat.getA(A);

    const mat_l& R = aMat.getR();
    #ifdef VERBOSE
    if (R.NumRows()<50 && R.NumCols()<50)
        cout << "\n R=" << R << endl;
    #endif

    mat_l RI(INIT_SIZE, m, n*k*e);
    for (long i=0; i<mBar; i++) for (long j=0; j<n*k*e; j++)
        {
            RI[i][j] = R[i][j];
        }
    for (long i=0; i<n*k*e; i++) for (long j=0; j<n*k*e; j++)
        {
            if (i==j) RI[i+mBar][j] = 1;
            else      RI[i+mBar][j] = 0;
        }

    #ifdef VERBOSE
    for (long i=0; i<k; i++)
    {
        params.zzp_context[i].restore();
        if (params.m < 50)
            cout << "\n A mod " << zz_p::modulus() << "=" << A[i] << endl;

        mat_zz_p A_RI;
        mul(A_RI, A[i], conv<mat_zz_p>(RI));
        if (params.m < 50)
            cout << "\n A*[R/I] mod " << zz_p::modulus() << "="
                 << A_RI << endl;
    }
    #endif

    // Compute a random syndrome
    Vec<vec_zz_p> syndrome(INIT_SIZE, k);
    for (long i=0; i<k; i++)
    {
        params.zzp_context[i].restore();
        syndrome[i].SetLength(n);
        for (long j=0; j<n; j++) syndrome[i][j] = random_zz_p();

        #ifdef VERBOSE
        if (params.m < 50)
            cout << "syndrome mod "
                 << power_long(factors[i],e)<<" ((factor "<<i<<")^"<<e+1<<")= "<<syndrome[i]<<endl <<flush;
        #endif
    }

    // Sample x such that A*x = syndrome
    Vec<long> x;
    aMat.sampleWithTrapdoor(x, syndrome);
    // vec_l p, z;
    // aMat.sampleWithTrapdoor(x, p, z, syndrome); // debugging version

    #ifdef VERBOSE
    if (params.m < 50) cout << "sampled vector="<<x<<endl;
    #endif

    assert(x.length()== m);
    // check that we have the right answer modulo p^e for all factors

    bool bSuccess = true;

    for (long i=0; i<k; i++)
    {
        params.zzp_context[i].restore();
        vec_zz_p xMod = conv<vec_zz_p>(x);
        vec_zz_p uu;
        mul(uu, A[i], xMod);
        if (syndrome[i]!=uu)
        {
            cout << "bit " << i << " failed!" << endl;
            bSuccess = false;

            //assert(syndrome[i]==uu);
        }
    }
    if (bSuccess == true)
        cout << "\nPASSED\n";
    cout << "maxSigma="<<Gaussian1Dsampler::maxSigma
         << ", maxSample="<<TDMatrix::maxSample << endl;

#ifdef DEBUG
    printAllTimers(cout);
#endif

#ifdef CodeBlocks
    cin.get();
#endif

    return 0;

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   testVector.cpp
 * @brief  Unit tests for Vector class
 * @author Frank Dellaert
 **/

#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>

using namespace std;
using namespace gtsam;

namespace {
  /* ************************************************************************* */
  template<typename Derived>
  Vector testFcn1(const Eigen::DenseBase<Derived>& in)
  {
    return in;
  }

  /* ************************************************************************* */
  template<typename Derived>
  Vector testFcn2(const Eigen::MatrixBase<Derived>& in)
  {
    return in;
  }
}

/* ************************************************************************* */
TEST(Vector, special_comma_initializer)
{
  Vector expected(3);
  expected(0) = 1;
  expected(1) = 2;
  expected(2) = 3;

  Vector actual1 = Vector3(1, 2, 3);
  Vector actual2(Vector3(1, 2, 3));

  Vector subvec1 = Vector2(2, 3);
  Vector actual4 = (Vector(3) << 1, subvec1).finished();

  Vector subvec2 = Vector2(1, 2);
  Vector actual5 = (Vector(3) << subvec2, 3).finished();

  Vector actual6 = testFcn1(Vector3(1, 2, 3));
  Vector actual7 = testFcn2(Vector3(1, 2, 3));

  EXPECT(assert_equal(expected, actual1));
  EXPECT(assert_equal(expected, actual2));
  EXPECT(assert_equal(expected, actual4));
  EXPECT(assert_equal(expected, actual5));
  EXPECT(assert_equal(expected, actual6));
  EXPECT(assert_equal(expected, actual7));
}

/* ************************************************************************* */
TEST(Vector, copy )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  double data[] = {10,20};
  Vector b(2);
  copy(data,data+2,b.data());
  EXPECT(assert_equal(a, b));
}

/* ************************************************************************* */
TEST(Vector, scalar_multiply )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = 1; b(1) = 2;
  EXPECT(assert_equal(a,b*10.0));
}

/* ************************************************************************* */
TEST(Vector, scalar_divide )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = 1; b(1) = 2;
  EXPECT(assert_equal(b,a/10.0));
}

/* ************************************************************************* */
TEST(Vector, negate )
{
  Vector a(2); a(0) = 10; a(1) = 20;
  Vector b(2); b(0) = -10; b(1) = -20;
  EXPECT(assert_equal(b, -a));
}

/* ************************************************************************* */
TEST(Vector, householder )
{
  Vector x(4);
  x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;

  Vector expected(4);
  expected(0) = 1.0; expected(1) = -0.333333; expected(2) = -1.66667; expected(3) = -0.333333;

  pair<double, Vector> result = house(x);

  EXPECT(result.first==0.5);
  EXPECT(equal_with_abs_tol(expected,result.second,1e-5));
}

/* ************************************************************************* */
TEST(Vector, concatVectors)
{
  Vector A(2);
  for(int i = 0; i < 2; i++)
    A(i) = i;
  Vector B(5);
  for(int i = 0; i < 5; i++)
    B(i) = i;

  Vector C(7);
  for(int i = 0; i < 2; i++) C(i) = A(i);
  for(int i = 0; i < 5; i++) C(i+2) = B(i);

  list<Vector> vs;
  vs.push_back(A);
  vs.push_back(B);
  Vector AB1 = concatVectors(vs);
  EXPECT(AB1 == C);

  Vector AB2 = concatVectors(2, &A, &B);
  EXPECT(AB2 == C);
}

/* ************************************************************************* */
TEST(Vector, weightedPseudoinverse )
{
  // column from a matrix
  Vector x(2);
  x(0) = 1.0; x(1) = 2.0;

  // create sigmas
  Vector sigmas(2);
  sigmas(0) = 0.1; sigmas(1) = 0.2;
  Vector weights = sigmas.array().square().inverse();

  // perform solve
  Vector actual; double precision;
  boost::tie(actual, precision) = weightedPseudoinverse(x, weights);

  // construct expected
  Vector expected(2);
  expected(0) = 0.5; expected(1) = 0.25;
  double expPrecision = 200.0;

  // verify
  EXPECT(assert_equal(expected,actual));
  EXPECT(std::abs(expPrecision-precision) < 1e-5);
}

/* ************************************************************************* */
TEST(Vector, weightedPseudoinverse_constraint )
{
  // column from a matrix
  Vector x(2);
  x(0) = 1.0; x(1) = 2.0;

  // create sigmas
  Vector sigmas(2);
  sigmas(0) = 0.0; sigmas(1) = 0.2;
  Vector weights = sigmas.array().square().inverse();
  // perform solve
  Vector actual; double precision;
  boost::tie(actual, precision) = weightedPseudoinverse(x, weights);

  // construct expected
  Vector expected(2);
  expected(0) = 1.0; expected(1) = 0.0;

  // verify
  EXPECT(assert_equal(expected,actual));
  EXPECT(std::isinf(precision));
}

/* ************************************************************************* */
TEST(Vector, weightedPseudoinverse_nan )
{
  Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
  Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
  Vector weights = sigmas.array().square().inverse();
  Vector pseudo; double precision;
  boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);

  Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
  EXPECT(assert_equal(expected, pseudo));
  DOUBLES_EQUAL(100, precision, 1e-5);
}

/* ************************************************************************* */
TEST(Vector, dot )
{
  Vector a = Vector3(10., 20., 30.);
  Vector b = Vector3(2.0, 5.0, 6.0);
  DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9);
}

/* ************************************************************************* */
TEST(Vector, axpy )
{
  Vector x = Vector3(10., 20., 30.);
  Vector y0 = Vector3(2.0, 5.0, 6.0);
  Vector y1 = y0, y2 = y0;
  y1 += 0.1 * x;
  y2.head(3) += 0.1 * x;
  Vector expected = Vector3(3.0, 7.0, 9.0);
  EXPECT(assert_equal(expected,y1));
  EXPECT(assert_equal(expected,Vector(y2)));
}

/* ************************************************************************* */
TEST(Vector, equals )
{
  Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan
  Vector v2 = (Vector(1) << 1.0).finished();
  double tol = 1.;
  EXPECT(!equal_with_abs_tol(v1, v2, tol));
}

/* ************************************************************************* */
TEST(Vector, greater_than )
{
  Vector v1 = Vector3(1.0, 2.0, 3.0),
       v2 = Z_3x1;
  EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
  EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}

/* ************************************************************************* */
TEST(Vector, linear_dependent )
{
  Vector v1 = Vector3(1.0, 2.0, 3.0);
  Vector v2 = Vector3(-2.0, -4.0, -6.0);
  EXPECT(linear_dependent(v1, v2));
}

/* ************************************************************************* */
TEST(Vector, linear_dependent2 )
{
  Vector v1 = Vector3(0.0, 2.0, 0.0);
  Vector v2 = Vector3(0.0, -4.0, 0.0);
  EXPECT(linear_dependent(v1, v2));
}

/* ************************************************************************* */
TEST(Vector, linear_dependent3 )
{
  Vector v1 = Vector3(0.0, 2.0, 0.0);
  Vector v2 = Vector3(0.1, -4.1, 0.0);
  EXPECT(!linear_dependent(v1, v2));
}

//******************************************************************************
TEST(Vector, IsVectorSpace) {
  BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
  BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
  typedef Eigen::Matrix<double,1,-1> RowVector;
  BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib  -o sint sint.cpp -lboost_system -lboost_filesystem -lboost_iostreams
#include <iostream>
#include <boost/format.hpp>

using namespace std;

class SInt {
public:
	static constexpr unsigned convert(int i)     { return (static_cast<unsigned>(i) << 1) ^ (i >> (sizeof(int)*8-1)); }
	static constexpr int convertBack(unsigned i) { return (i >> 1) ^ -static_cast<int>(i & 1); }
	explicit SInt(const int& i) : v_{convert(i)}{}
	operator unsigned() const noexcept { return v_; }
private:
	unsigned v_;
};

template<typename T>
struct Str2Bin {
	explicit Str2Bin(const T& t) : t_{t}{}
	const volatile T& t_;
};

template<typename T>
ostream& operator<<(ostream& o, const Str2Bin<T>& s) {
	T t = s.t_;
	for(unsigned i = 0; i < sizeof(T) * 8; ++i, t<<=1) {
		if (i>0 && i % 8 == 0)
			o << '.';
		o << (t & (1<<(sizeof(T)*8-1) ) ? '1' : '0');
	}
	return o;
}

int main (int argc, char const *argv[])
{
	int ret = 0;
	int max = 255;
	for(int i = -max; i <= max; ++i) {
		SInt s(i);
		int j = SInt::convertBack(s);
		cout << boost::format{"%3d _ "} % i << Str2Bin<int>(i)
			 << boost::format{" _ %3d _ "} % s
			<< Str2Bin<unsigned>(s) << boost::format{" _ %3d _ "} % j
			<< Str2Bin<int>(j) << endl;
	}
	return ret;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// 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)

#include <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/first_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/bool.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::first_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;

using boost::mpl::apply_wrap1;
using boost::mpl::fold;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::eval_if;
using boost::mpl::bool_;
using boost::mpl::equal_to;
using boost::mpl::bool_;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= int_token ((mult_token | div_token) int_token)*
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct eval_plus
{
  template <class State, class C>
  struct apply :
    eval_if<
      is_c<front<C>, '+'>,
      plus<typename State::type, typename back<C>::type>,
      minus<typename State::type, typename back<C>::type>
    >
  {};
};

struct eval_mult
{
  template <class State, class C>
  struct apply :
    eval_if<
      is_c<front<C>, '*'>,
      times<typename State::type, typename back<C>::type>,
      divides<typename State::type, typename back<C>::type>
    >
  {};
};

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, int_token>,
    int_token,
    eval_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    eval_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > calculator_parser;

#ifdef _STR
#  error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

#ifdef BOOST_NO_CXX11_CONSTEXPR
int main()
{
  using std::cout;
  using std::endl;
  using boost::metaparse::string;

  cout
    << apply_wrap1<calculator_parser, string<'1','3'> >::type::value << endl
    <<
      apply_wrap1<
        calculator_parser, string<' ','1','+',' ','2','*','4','-','6','/','2'>
      >::type::value
    << endl
    ;
}
#else
int main()
{
  using std::cout;
  using std::endl;

  cout
    << apply_wrap1<calculator_parser, _STR("13")>::type::value << endl
    << apply_wrap1<calculator_parser, _STR(" 1+ 2*4-6/2")>::type::value << endl
    ;
}
#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/geometries/point.hpp>

#include <boost/geometry/util/algorithm.hpp>

void test_dimension(bg::util::index_constant<0> index)
{
    bool called = false;
    bg::detail::for_each_index<0>([&](auto index) { called = true; });
    BOOST_CHECK(!called);
    BOOST_CHECK(bg::detail::all_indexes_of<0>([&](auto index) { return true; }) == true);
    BOOST_CHECK(bg::detail::all_indexes_of<0>([&](auto index) { return false; }) == true);
    BOOST_CHECK(bg::detail::any_index_of<0>([&](auto index) { return true; }) == false);
    BOOST_CHECK(bg::detail::any_index_of<0>([&](auto index) { return false; }) == false);
    BOOST_CHECK(bg::detail::none_index_of<0>([&](auto index) { return true; }) == true);
    BOOST_CHECK(bg::detail::none_index_of<0>([&](auto index) { return false; }) == true);
}

template <std::size_t I>
void test_dimension(bg::util::index_constant<I>)
{
    using point = bg::model::point<double, I, bg::cs::cartesian>;
    point p;
    bg::assign_value(p, 10.0);

    bg::detail::for_each_index<I>([&](auto index)
    {
        BOOST_CHECK(bg::get<index>(p) == 10.0);
        bg::set<index>(p, double(index));
    });
    bg::detail::for_each_dimension<point>([&](auto index)
    {
        BOOST_CHECK(bg::get<index>(p) == double(index));
    });

    BOOST_CHECK(
        bg::detail::all_indexes_of<0>([&](auto index)
        {
            return bg::get<index>(p) == double(index);
        }) == true);
    BOOST_CHECK(
        bg::detail::all_dimensions_of<point>([&](auto index)
        {
            return bg::get<index>(p) == 10;
        }) == false);
    BOOST_CHECK(
        bg::detail::any_index_of<0>([&](auto index)
        {
            return false;
        }) == false);
    BOOST_CHECK(
        bg::detail::any_dimension_of<point>([&](auto index)
        {
            return bg::get<index>(p) == double(I - 1);
        }) == true);
    BOOST_CHECK(
        bg::detail::none_index_of<0>([&](auto index)
        {
            return false;
        }) == true);
    BOOST_CHECK(
        bg::detail::none_dimension_of<point>([&](auto index)
        {
            return bg::get<index>(p) == double(0);
        }) == false);
}

template <std::size_t I, std::size_t N>
struct test_dimensions
{
    static void apply()
    {
        test_dimension(bg::util::index_constant<I>());
        test_dimensions<I + 1, N>::apply();
    }
};

template <std::size_t N>
struct test_dimensions<N, N>
{
    static void apply() {}
};

int test_main(int, char* [])
{
    test_dimensions<0, 5>::apply();

    return 0;
}


/*
 * File:   random.cc
 * Author: ts337
 *
 * Created on 28 February 2011, 11:58
 */

#include "../random.h"
#include <boost/random/poisson_distribution.hpp>

#include <unuran.h>
#include <unuran_urng_rngstreams.h>
#include <vector>
#include <stdexcept>
#include <string>

using std::string;
using std::logic_error;
using std::to_string;

namespace coela {
//=========================================================================================================
namespace boost_random {
UniformRandomVariate::UniformRandomVariate(unsigned int seed):
    rng(seed), dist(0,1), rv(rng,dist) {}
} //end namespace coela::boost_random
//=========================================================================================================
//=========================================================================================================
namespace unuran {

int StreamWrapper::num_streams_created=0;
bool StreamWrapper::package_seed_set=false;

void StreamWrapper::set_unuran_package_seed(
    std::vector<unsigned long>& six_seed_numbers,
    bool check_if_already_set
)
{
    if (six_seed_numbers.size()!=6) {
        throw logic_error("random_number_stream::set_unuran_package_seed - "
                            "please supply vector of six numbers");
    }

    if (check_if_already_set && unuran_package_has_been_seeded()) {
        throw logic_error("random_number_stream::set_unuran_package_seed - "
                            "package seed already set");
    }

    RngStream_SetPackageSeed(& six_seed_numbers[0]);

    package_seed_set=true;

}

void StreamWrapper::advance_package_seed(const size_t n_steps)
{
    for (size_t i=0; i!=n_steps; ++i) {
        StreamWrapper temp_instantiation_to_force_seed_advance;
    }
    return;
}

StreamWrapper::StreamWrapper()
{
    if (!package_seed_set) throw logic_error(
            "random_number_stream::random_number_stream() - please set package seed first");

    rn_stream_ob_ptr = unur_urng_rngstream_new(string("urng-"+
            to_string(num_streams_created)).c_str());

    if (rn_stream_ob_ptr == NULL) throw std::runtime_error(
            "random_number_stream::random_number_stream - could not create new rng stream");
    num_streams_created++;
}

StreamWrapper::~StreamWrapper()
{
    unur_urng_free(rn_stream_ob_ptr);
}

//=========================================================================================================
UniformRandomVariate::UniformRandomVariate(double lower_bound, double upper_bound,
        StreamWrapper& rns)
    :gen(NULL)
{

    assert(lower_bound < upper_bound);
    UNUR_DISTR *distr;    /* distribution object                   */
    UNUR_PAR   *par;      /* parameter object                      */
    double urv_params[2];
    urv_params[0]=lower_bound;
    urv_params[1]=upper_bound;
    distr = unur_distr_uniform(urv_params, 2);
    par = unur_cstd_new(distr);
    unur_set_urng(par, rns.get_stream_pointer());
    gen = unur_init(par);
    unur_distr_free(distr);

    if (gen==NULL) { throw std::runtime_error("uniform_random_variate (constructor) - could not construct generator."); }

}
UniformRandomVariate::~UniformRandomVariate()
{
    unur_free(gen);
}

double UniformRandomVariate::operator()()
{
    return unur_sample_cont(gen);
}

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

GaussianRandomVariate::GaussianRandomVariate(double mean, double sigma,
        StreamWrapper& wrapped_rng)
    :gen(NULL)
{
    UNUR_DISTR *distr;    /* distribution object                   */
    UNUR_PAR   *par;      /* parameter object                      */
    double gaussian_params[2];
    gaussian_params[0]=mean;
    gaussian_params[1]=sigma;
    distr = unur_distr_normal(gaussian_params, 2);
    par = unur_cstd_new(distr);

    // Choose generation method: http://statmath.wu.ac.at/unuran/doc/unuran.html#normal
    //TO DO: Profile which is faster for likely use case of single generation per instance.
//    unur_cstd_set_variant(par, 2);

    unur_set_urng(par, wrapped_rng.get_stream_pointer());
    gen = unur_init(par);
    unur_distr_free(distr);

    if (gen==NULL) { throw std::runtime_error("gaussian_random_variate::gaussian_random_variate - could not construct generator."); }
}

GaussianRandomVariate::~GaussianRandomVariate()
{
    unur_free(gen);
}

//void gaussian_random_variate::update_params(double mean, double sigma){
//    double gaussian_params[2];
//    gaussian_params[0]=mean;
//    gaussian_params[1]=sigma;
//
//    unur_distr_cont_set_pdfparams(unur_get_distr(gen), gaussian_params, 2);
//    unur_distr_cont_upd_mode(unur_get_distr(gen));
//    unur_distr_cont_upd_pdfarea(unur_get_distr(gen));
//    if ( unur_reinit(gen) ) throw runtime_error("Gaussian_random_variate::update_params - threw an error code");
//}

double GaussianRandomVariate::operator()()
{
    return unur_sample_cont(gen);
}

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

// Use default random number stream- deprecated to encourage good use of seeds
//poisson_random_variate::poisson_random_variate(double lambda)
//:gen(NULL)
//{
//    UNUR_DISTR *distr;    /* distribution object                   */
//    UNUR_PAR   *par;      /* parameter object                      */
//    double poisson_params[1];
//    poisson_params[0]=lambda;
//    distr = unur_distr_poisson(poisson_params, 1);
//    par = unur_auto_new(distr);
//    gen = unur_init(par);
//    unur_distr_free(distr);
//
//    if (gen==NULL) throw std::runtime_error("simple_poisson_random_variate::simple_poisson_random_variate - could not construct generator.");
// }

PoissonRandomVariate::PoissonRandomVariate(double lambda,
        StreamWrapper& wrapped_rng)
    :gen(NULL)
{
    UNUR_DISTR *distr;    /* distribution object                   */
    UNUR_PAR   *par;      /* parameter object                      */
    double poisson_params[1];
    poisson_params[0]=lambda;
    distr = unur_distr_poisson(poisson_params, 1);
    par = unur_dstd_new(distr);
    unur_set_urng(par, wrapped_rng.get_stream_pointer());
    gen = unur_init(par);
    unur_distr_free(distr);

    if (gen==NULL) {
        throw std::runtime_error(
            "simple_poisson_random_variate - could not construct generator.");
    }
}

PoissonRandomVariate::~PoissonRandomVariate()
{
    unur_free(gen);
}

int PoissonRandomVariate::operator()()
{
    return unur_sample_discr(gen);
}

//=========================================================================================================
}//end namespace coela::unuran
}//end namespace coela


/// random.hpp
/// eigen
///
/// Purpose:
/// Define randomization functions used in Eigen operators
///

#ifndef GLOBAL_RANDOM_HPP
#define GLOBAL_RANDOM_HPP

#include <random>
#include <type_traits>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

#include "internal/global/config.hpp"

namespace global
{

/// Function that returns a generated value
template <typename T>
using GenF = std::function<T()>;

struct iGenerator
{
	virtual ~iGenerator (void) = default;

	/// Return random string
	virtual std::string get_str (void) const = 0;

	/// Return uniformly generate integer between a and b
	virtual int64_t unif_int (
		const int64_t& lower, const int64_t& upper) const = 0;

	/// Return uniformly generate decimal between a and b
	virtual double unif_dec (
		const double& lower, const double& upper) const = 0;

	/// Return normally generate decimal with mean and stdev
	virtual double norm_dec (
		const double& mean, const double& stdev) const = 0;

	/// Return random string generator function
	virtual GenF<std::string> get_strgen (void) const = 0;

	/// Return generator function that uniformly generates integers between a and b
	virtual GenF<int64_t> unif_intgen (
		const int64_t& lower, const int64_t& upper) const = 0;

	/// Return generator function that uniformly generates decimals between a and b
	virtual GenF<double> unif_decgen (
		const double& lower, const double& upper) const = 0;

	/// Return generator function that normally generate decimal with mean and stdev
	virtual GenF<double> norm_decgen (
		const double& mean, const double& stdev) const = 0;
};

using GenPtrT = std::shared_ptr<iGenerator>;

struct iRandGenerator : public iGenerator
{
	virtual ~iRandGenerator (void) = default;

	/// Seed the random engine using seed specified
	virtual void seed (size_t s) = 0;
};

void set_generator (GenPtrT gen, CfgMapptrT ctx = context());

GenPtrT get_generator (const CfgMapptrT& ctx = context());

void seed (size_t s, const CfgMapptrT& ctx = context());

struct Randomizer final : public iRandGenerator
{
	/// Implementation of iGenerator
	std::string get_str (void) const override
	{
		return boost::uuids::to_string(uengine_());
	}

	/// Implementation of iGenerator
	int64_t unif_int (const int64_t& lower, const int64_t& upper) const override
	{
		std::uniform_int_distribution<int64_t> dist(lower, upper);
		return dist(rengine_);
	}

	/// Implementation of iGenerator
	double unif_dec (const double& lower, const double& upper) const override
	{
		std::uniform_real_distribution<double> dist(lower, upper);
		return dist(rengine_);
	}

	/// Implementation of iGenerator
	double norm_dec (const double& mean, const double& stdev) const override
	{
		std::normal_distribution<double> dist(mean, stdev);
		return dist(rengine_);
	}

	/// Implementation of iGenerator
	GenF<std::string> get_strgen (void) const override
	{
		return [this]{ return boost::uuids::to_string(this->uengine_()); };
	}

	/// Implementation of iGenerator
	GenF<int64_t> unif_intgen (
		const int64_t& lower, const int64_t& upper) const override
	{
		std::uniform_int_distribution<int64_t> dist(lower, upper);
		return std::bind(dist, rengine_);
	}

	/// Implementation of iGenerator
	GenF<double> unif_decgen (
		const double& lower, const double& upper) const override
	{
		std::uniform_real_distribution<double> dist(lower, upper);
		return std::bind(dist, rengine_);
	}

	/// Implementation of iGenerator
	GenF<double> norm_decgen (
		const double& mean, const double& stdev) const override
	{
		std::normal_distribution<double> dist(mean, stdev);
		return std::bind(dist, rengine_);
	}

	/// Implementation of iRandGenerator
	void seed (size_t s) override
	{
		rengine_.seed(s);
	}

	mutable std::default_random_engine rengine_;

	mutable boost::uuids::random_generator uengine_;
};

}

#endif // GLOBAL_RANDOM_HPP


/*
 * mapr_project.cpp
 *
 *  Created on: June 15, 2020
 *      Author: Kamil Miedzinski Mateusz Grycmacher
 *	 Institute: Instute of Home, Poznan University of Technology
 */

#include "../include/mapr_project/mapr_project.hpp"

// Boost
#include <boost/bind.hpp>
#include <boost/thread/recursive_mutex.hpp>

// STL
#include <string>
#include <math.h>
#include <limits>
#include <thread>

using namespace std;
using namespace ros;

namespace ob = ompl::base;
namespace og = ompl::geometric;

namespace mapr_project {

grid_map_msgs::GridMap gridMap;

double point_start_x = -0.5;
double point_start_y = -0.5;
double point_end_x = -5.0;
double point_end_y = -5.0;

class ValidityChecker : public ob::StateValidityChecker
{
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}

    bool isValid(const ob::State* state) const
    {
        return this->clearance(state);
    }

    double clearance(const ob::State* state) const
    {
        const ob::RealVectorStateSpace::StateType* state2D =
            state->as<ob::RealVectorStateSpace::StateType>();
        // Extract the robot's (x,y) position from its state
        double x = state2D->values[0];
        double y = state2D->values[0];
        int col = (x)/(-0.1);
     	int row = (y)/(-0.1);

	float resolution = gridMap.info.resolution;

  	float point_start_z = 0;
	float point_end_z = 0;

  	if(gridMap.info.length_x>0)  // Mapa wysokosci subskyrbowana
	{
		// Wysokosc z jakiej startujemy i do jakiej zmierzamy
		point_start_z = gridMap.data[0].data[(point_start_x/resolution*(-1)) + (point_start_y/resolution*(-1)) * 64];
		point_end_z = gridMap.data[0].data[(point_end_x/resolution*(-1)) + (point_end_y/resolution*(-1)) * 64];
		//std::cout << "point_start_z: " << point_start_z << " point_end_z: " << point_end_z << "\n";

		// Idziemy z gorki
		if(point_start_z > point_end_z)
		{
			if ((gridMap.data[0].data[col + row * 64]<point_start_z) && (gridMap.data[0].data[col + row * 64]>point_end_z))
	    		{

				return gridMap.data[0].data[col + row * 64];
	 		}
		 	else
			{
				return gridMap.data[0].data[col + row * 64]*gridMap.data[0].data[col + row * 64];
			}
		}
		else // Idziemy pod gore
		{
			if ((gridMap.data[0].data[col + row * 64]>point_start_z) && (gridMap.data[0].data[col + row * 64]<point_end_z))
	    		{
				return gridMap.data[0].data[col + row * 64];
	 		}
		 	else
			{
				return gridMap.data[0].data[col + row * 64]*gridMap.data[0].data[col + row * 64];
			}

		}
	}
	else // Brak mapy
	{
		return 1;
	}
    }
};

class ClearanceObjective : public ob::StateCostIntegralObjective
{
public:
    ClearanceObjective(const ob::SpaceInformationPtr& si) :
        ob::StateCostIntegralObjective(si, true)
    {
    }
    ob::Cost stateCost(const ob::State* s) const
    {
        return ob::Cost(si_->getStateValidityChecker()->clearance(s));
    }
};

//Optimization - getPathLengthObjective
 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
 {
     return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
 }

//Optimization - getClearanceObjective
ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
{
    return std::make_shared<ClearanceObjective>(si);

}

//Optimization - getBalancedObjective
ob::OptimizationObjectivePtr getBalancedObjective(const ob::SpaceInformationPtr& si)
{
    ob::OptimizationObjectivePtr lengthObj(new ob::PathLengthOptimizationObjective(si));
    ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
    ob::MultiOptimizationObjective* opt = new ob::MultiOptimizationObjective(si);
    opt->addObjective(lengthObj, 1.0);
    opt->addObjective(clearObj, 10.0);
    return ob::OptimizationObjectivePtr(opt);
}

Planner2D::Planner2D(ros::NodeHandle& _nodeHandle)
    : nodeHandle(_nodeHandle)
{
    ROS_INFO("Planner node started.");
    configure(point_start_x, point_start_y, point_end_x, point_end_y);
}

Planner2D::~Planner2D()
{
}

void Planner2D::returnPoints(std_msgs::UInt8 pStartX, std_msgs::UInt8 pStartY,
                             std_msgs::UInt8 pEndX, std_msgs::UInt8 pEndY){
    point_start_x = double(pStartX.data) * (-0.1);
    point_start_y = double(pStartY.data) * (-0.1);
    point_end_x = double(pEndX.data) * (-0.1);
    point_end_y = double(pEndY.data) * (-0.1);
}

/// extract path
nav_msgs::Path Planner2D::extractPath(ob::ProblemDefinition* pdef){
   nav_msgs::Path plannedPath;
    plannedPath.header.frame_id = "/map";
    // get the obtained path
    ob::PathPtr path = pdef->getSolutionPath();
    // print the path to screen
    path->print(std::cout);
    // convert to geometric path
    const auto *path_ = path.get()->as<og::PathGeometric>();
    // iterate over each position
    for(unsigned int i=0; i<path_->getStateCount(); ++i){
        // get state
        const ob::State* state = path_->getState(i);

	const ob::RealVectorStateSpace::StateType* state2D = state->as<ob::RealVectorStateSpace::StateType>();

        // Extract the robot's (x,y) position from its state
        double x = state2D->values[0];
        double y = state2D->values[1];
	// potrzebne do obliczania wspl. Z sciezki
	int col = (x)/(-0.1);
     	int row = (y)/(-0.1);

        // fill in the ROS PoseStamped structure...
        geometry_msgs::PoseStamped poseMsg;
        poseMsg.pose.position.x = x;
        poseMsg.pose.position.y = y;
	if (gridMap.info.length_x)
    	{
		poseMsg.pose.position.z = gridMap.data[0].data[col + row * 64] +0.1;
 	}
	else
	{
		poseMsg.pose.position.z = 0;
	}
        poseMsg.pose.orientation.w = 1.0;
        poseMsg.pose.orientation.x = 0.0;
        poseMsg.pose.orientation.y = 0.0;
        poseMsg.pose.orientation.z = 0.0;
        poseMsg.header.frame_id = "/map";
        poseMsg.header.stamp = ros::Time::now();
        // ... and add the pose to the path
        plannedPath.poses.push_back(poseMsg);
    }
    return plannedPath;
}

nav_msgs::Path Planner2D::planPath(const grid_map_msgs::GridMap& globalMap){

    	gridMap = globalMap;
	configure(point_start_x, point_start_y, point_end_x, point_end_y);

	//std::cout << "point_start_x" << point_start_x << std::endl;
	//std::cout << "point_start_y" << point_start_y << std::endl;

   	 // Construct the robot state space in which we're planning. We're
	// planning in [0,1]x[0,1], a subset of R^2.
	ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));

	// Set the bounds of space to be in [0,1].
	space->as<ob::RealVectorStateSpace>()->setBounds(-6.4, 0.0);

	// Construct a space information instance for this state space
	ob::SpaceInformationPtr si(new ob::SpaceInformation(space));

	 // Set the object used to check which states in the space are valid
	si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
	si->setup();

	// Set our robot's starting state to be the bottom-left corner of
	// the environment, or (0,0).
	ob::ScopedState<> start(space);
	start->as<ob::RealVectorStateSpace::StateType>()->values[0] = point_start_x;
	start->as<ob::RealVectorStateSpace::StateType>()->values[1] = point_start_y;

	// Set our robot's goal state to be the top-right corner of the
	// environment, or (5,5).
	ob::ScopedState<> goal(space);
	goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = point_end_x;
	goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = point_end_y;

	// Create a problem instance
	ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));

	// Set the start and goal states
	pdef->setStartAndGoalStates(start, goal);

	// Create the optimization objective
	pdef->setOptimizationObjective(getBalancedObjective(si));

	// Construct our optimizing planner using the RRTstar algorithm.
	auto optimizingPlanner(std::make_shared<og::RRTstar>(si));
	optimizingPlanner->setRange(maxStepLength);// max step length

	// Set the problem instance for our planner to solve
	optimizingPlanner->setProblemDefinition(pdef);
	optimizingPlanner->setup();

	// attempt to solve the planning problem within one second of
	// planning time
	ob::PlannerStatus solved = optimizingPlanner->ob::Planner::solve(1.0);

 	nav_msgs::Path plannedPath;
	 if (solved)
 	    {
  	    	plannedPath=extractPath(pdef.get());
 	    }
  	   else
	    {
  	       std::cout << "No solution found." << std::endl;
	    }

	 return plannedPath;
}

/// configure planner
void Planner2D::configure(double point_start_x, double point_start_y, double point_end_x, double point_end_y)
{
        maxStepLength = 0.05;// max step length
}

} /* namespace */


#include <boost/test/unit_test.hpp>
#include "functions/unary_minus.hh"
#include "functions/full_function_defs.hh"
#include "functions/variables.hh"

BOOST_AUTO_TEST_CASE(unary_minus_test) {
  using namespace manifolds;
  UnaryMinus<decltype(x)> a(x);

  BOOST_CHECK_EQUAL(a(1, 2, 3, 4), -1);
  BOOST_CHECK_EQUAL(a(1, 2, 4, 8, 16), -1);
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "modules/geometry/polygon.hpp"
#include "modules/geometry/line.hpp"
#include "modules/geometry/commons.hpp"
#include "modules/models/dynamic/single_track.hpp"
#include "modules/models/dynamic/triple_integrator.hpp"
#include "modules/models/dynamic/integration.hpp"
#include "modules/commons/params/setter_params.hpp"
#include "modules/commons/params/default_params.hpp"

TEST(single_track_model, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;
  using namespace modules::commons;

  State x(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  x << 0, 0, 0, 0, 5;

  Input u(2);
  u << 0, 0;

  DynamicModel *m;
  auto params = std::make_shared<DefaultParams>();
  SingleTrackModel single_track_model(params);
  m = &single_track_model;

  float dt = 0.1;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u, dt);
    cout << x << endl;
  }
}

TEST(triple_integrator_model, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;
  using namespace modules::commons;

  State x(15);
  x << 0, 0, 0, 0, 0, 0,  // time, x, y, theta, v, min_space
       0, 1, 0,  // x, vx, ax
       0, 1, 0,  // y, vy, ay
       0, 1, 0;  // z, vz, az

  Input u0(3);
  u0 << 0, 0, 0.;

  DynamicModel *m;
  auto params = std::make_shared<DefaultParams>();
  TripleIntegratorModel triple_int_model(params);
  m = &triple_int_model;

  float dt = 0.1;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u0, dt);
    cout << x << endl << endl;
  }
  // TODO(@hart): assert state
  u0 << 1., 1., 1.;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u0, dt);
    cout << x << endl << endl;
  }
  // TODO(@hart): assert state

}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file tree_augmentation_example.cpp
 * @brief
 * @author Attila Bernath, Piotr Godlewski
 * @version 1.0
 * @date 2013-10-17
 */

//! [Tree Augmentation Example]
#include "paal/iterative_rounding/treeaug/tree_augmentation.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <iostream>
#include <vector>

int main() {
    using EdgeProp = boost::property<boost::edge_weight_t, double,
                boost::property<boost::edge_color_t, bool>>;
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
                boost::undirectedS, boost::no_property, EdgeProp>;
    using Edge = boost::graph_traits<Graph>::edge_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges {{0,1},{1,2},{1,3},{3,4},{3,5},
            {0,3},{0,3},{2,4},{2,5},{4,5}};
    std::vector<EdgeProp> edge_properties {EdgeProp(0, true),
        EdgeProp(0, true), EdgeProp(0, true), EdgeProp(0, true),
        EdgeProp(0, true), EdgeProp(1, false), EdgeProp(1, false),
        EdgeProp(1, false), EdgeProp(1, false), EdgeProp(1,false)};

    Graph g(edges.begin(), edges.end(), edge_properties.begin(), 6);

    std::vector<Edge> solution;

    // optional input validity checking
    auto tree_aug = paal::ir::make_tree_aug(g, std::back_inserter(solution));
    auto error = tree_aug.check_input_validity();
    if (error) {
        std::cerr << "The input is not valid!" << std::endl;
        std::cerr << *error << std::endl;
        return -1;
    }

    // solve it
    auto result = paal::ir::tree_augmentation_iterative_rounding(
        g, std::back_inserter(solution));

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "The solution contains the following nontree edges:"
            << std::endl;
        for (auto e : solution) {
            std::cout << "Edge " << e << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
    //! [Tree Augmentation Example]


/*
 * Copyright (C) 2018-2020 HERE Europe B.V.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 * License-Filename: LICENSE
 */

//
// Created by Mitra, Aniket on 2019-04-03.
//

#include "movetk/utils/GeometryBackendTraits.h"
#include "movetk/utils/Iterators.h"
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// treat a trajectory as a directed graph and
// use the BFS algorithm to find the shortest path
int main(int argc, char **argv)
{
#if CGAL_BACKEND_ENABLED
    std::cerr << "Using CGAL Backend for Geometry\n";
#else
    std::cerr << "Using Boost Backend for Geometry\n";
#endif

    std::cout.setf(std::ios::fixed);

    // PolyLine is a data structure that to store a collection of points
    typedef std::vector<GeometryKernel::MovetkGeometryKernel::MovetkPoint> PolyLine;

    // Define a graph using boost::adjacency_list
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                                  boost::property<boost::vertex_index_t, int>, boost::property<boost::edge_index_t, int>>
        Graph;

    // Vertex iterator type
    typedef typename boost::graph_traits<Graph>::vertex_iterator vertex_iterator;
    // Edge iterator type
    typedef typename boost::graph_traits<Graph>::edge_iterator edge_iterator;

    typedef boost::property_map<Graph, boost::vertex_index_t>::type VertexId_PMap;

    // functor to create a point
    movetk_core::MakePoint<GeometryKernel::MovetkGeometryKernel> make_point;

    // create polyline, each point in the polyline is vertex
    PolyLine polyline({make_point({-6.19, -3.46}), make_point({-4.99, 1.16}),
                       make_point({-2.79, -2.22}), make_point({-1.87, 0.58})});

    // print each vertex
    std::cout << "Polyline: ";
    std::cout << "{";
    for (auto &vertex : polyline)
    {
        std::cout << vertex;
        std::cout << ";";
    }
    std::cout << "}\n";

    // edges to connect vertex pairs
    std::vector<std::pair<std::size_t, std::size_t>> edges({
        std::make_pair(0, 1),
        std::make_pair(1, 2),
        std::make_pair(0, 2),
        std::make_pair(2, 3),
    });

    // print the edges
    std::cout << "Edges: ";
    std::cout << "{";
    for (auto &i : edges)
    {
        std::cout << i.first << "," << i.second << ";";
    }
    std::cout << "}\n";

    /** Pass iterator over edges and the number of vertices
     *  to create boost graph
    */
    Graph g{edges.begin(), edges.end(), polyline.size()};

    std::cout << "Number of vertices: " << boost::num_vertices(g) << '\n';
    std::cout << "Number of edges: " << boost::num_edges(g) << '\n';

    // container stores computed distances
    std::vector<std::size_t> distances(polyline.size());
    //
    std::vector<std::size_t> predecessors(polyline.size());

    predecessors[0] = 0;

    VertexId_PMap vertex_index = boost::get(boost::vertex_index, g);

    // iterator property map for recording distances
    boost::iterator_property_map<std::vector<std::size_t>::iterator, VertexId_PMap,
                                 std::size_t, std::size_t &>
        distances_pa(distances.begin(), vertex_index);

    /* iterator property map for recording the previous point as the
    * bfs algorithm proceeds to the next point
    */
    boost::iterator_property_map<std::vector<std::size_t>::iterator, VertexId_PMap,
                                 std::size_t, std::size_t &>
        predecessors_pa(predecessors.begin(), vertex_index);

    /** the helper function to create a visitor that record distances
    * distance is the number of lines that have to be crossed to get from one
    * point to another starting from the point that is passed as a second
    * argument to  boost::breadth_first_search
    */
    auto distance_recorder = boost::record_distances(distances_pa, boost::on_tree_edge{});
    /* returns a visitor to store the predecessor of every point
    *  whenever boost::breadth_first_search() visits a new point, the previous
    *  point is stored in the property map passed to boost::record_predecessors
    */
    auto predecessor_recorder = boost::record_predecessors(predecessors_pa, boost::on_tree_edge{});
    /* an adaptor that binds the two visitors distance_recorder & predecessor_recorder
    * to the algorithm
    */
    auto bfs_visitor = boost::make_bfs_visitor(std::make_pair(distance_recorder, predecessor_recorder));

    // the bfs algorithm on graph g
    // bfs_visitor stores the result
    // the algorithm starts at the first point as specified in the second argument
    boost::breadth_first_search(g, 0, boost::visitor(bfs_visitor));

    /* iterate over the graph and print all vertices, the distance
    * between two vertices, and the predecessor for each vertex
    */
    vertex_iterator vi, vi_end;
    for (std::tie(vi, vi_end) = boost::vertices(g); vi != vi_end; ++vi)
    {
        std::cout << "vertex index: " << *vi << ", distance: " << boost::get(distances_pa, *vi) << std::endl;
    }

    for (std::tie(vi, vi_end) = boost::vertices(g); vi != vi_end; ++vi)
    {
        std::cout << "vertex index: " << *vi << ", predecessor: " << boost::get(predecessors_pa, *vi) << std::endl;
    }

    /* get the index of the vertices returned by the
    * shortest path
    */
    std::tie(vi, vi_end) = boost::vertices(g);
    auto idx = boost::get(predecessors_pa, *(vi_end - 1));
    std::vector<std::size_t> indexes;
    indexes.push_back(*(vi_end - 1));
    while (idx != boost::get(predecessors_pa, *vi))
    {
        indexes.push_back(idx);
        idx = predecessors_pa[idx];
    }
    indexes.push_back(idx);

    /* the order of the vertices returned by the bfs algorithm
    * starts with the last vertex, so reverse the indexes
    * so that it can be used to access points from the polyline
    */
    std::reverse(std::begin(indexes), std::end(indexes));

    std::cout << "Shortest Path Polyline: ";
    std::cout << "{";
    for (auto &idx : indexes)
    {
        std::cout << polyline[idx] << ";";
    }
    std::cout << "}\n";
}


/*
 * mean_and_variance.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/image/filter/mean_and_variance.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  template <typename FloatType>
  void mean_and_variance_filter_wrapper(const char *name) {
    typedef MeanAndVarianceFilter<FloatType> MeanAndVarianceFilterType;

    class_<MeanAndVarianceFilterType>(name, no_init)
      .def(init<const af::const_ref<FloatType, af::c_grid<2> > &, int2>(
        (arg("image"), arg("size"))))
      .def("mean", &MeanAndVarianceFilterType::mean)
      .def("variance", &MeanAndVarianceFilterType::variance)
      .def("sample_variance", &MeanAndVarianceFilterType::sample_variance);
  }

  template <typename FloatType>
  void mean_and_variance_filter_masked_wrapper(const char *name) {
    typedef MeanAndVarianceFilterMasked<FloatType> MeanAndVarianceFilterType;

    class_<MeanAndVarianceFilterType>(name, no_init)
      .def(init<const af::const_ref<FloatType, af::c_grid<2> > &,
                const af::const_ref<int, af::c_grid<2> > &,
                int2,
                int>((arg("image"), arg("mask"), arg("size"), arg("min_size"))))
      .def("mean", &MeanAndVarianceFilterType::mean)
      .def("variance", &MeanAndVarianceFilterType::variance)
      .def("sample_variance", &MeanAndVarianceFilterType::sample_variance)
      .def("mask", &MeanAndVarianceFilterType::mask)
      .def("count", &MeanAndVarianceFilterType::count);
  }

  template <typename FloatType>
  MeanAndVarianceFilter<FloatType> make_mean_and_variance_filter(
    const af::const_ref<FloatType, af::c_grid<2> > &image,
    int2 size) {
    return MeanAndVarianceFilter<FloatType>(image, size);
  }

  template <typename FloatType>
  MeanAndVarianceFilterMasked<FloatType> make_mean_and_variance_filter_masked(
    const af::const_ref<FloatType, af::c_grid<2> > &image,
    const af::const_ref<int, af::c_grid<2> > &mask,
    int2 size,
    int min_size) {
    return MeanAndVarianceFilterMasked<FloatType>(image, mask, size, min_size);
  }

  template <typename FloatType>
  void mean_and_variance_filter_suite() {
    def("mean_filter", &mean_filter<FloatType>, (arg("image"), arg("size")));

    def("mean_filter",
        &mean_filter_masked<FloatType>,
        (arg("image"),
         arg("mask"),
         arg("size"),
         arg("min_count"),
         arg("ignore_masked") = true));

    def("mean_and_variance_filter",
        &make_mean_and_variance_filter<FloatType>,
        (arg("image"), arg("kernel")));

    def("mean_and_variance_filter",
        &make_mean_and_variance_filter_masked<FloatType>,
        (arg("image"), arg("mask"), arg("kernel"), arg("min_count")));
  }

  void export_mean_and_variance() {
    mean_and_variance_filter_wrapper<float>("MeanAndVarianceFilterFloat");
    mean_and_variance_filter_wrapper<double>("MeanAndVarianceFilterDouble");
    mean_and_variance_filter_masked_wrapper<float>("MeanAndVarianceFilterMaskedFloat");
    mean_and_variance_filter_masked_wrapper<double>(
      "MeanAndVarianceFilterMaskedDouble");

    mean_and_variance_filter_suite<float>();
    mean_and_variance_filter_suite<double>();
  }

}}}  // namespace dials::algorithms::boost_python


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include "votca/xtp/basisset.h"
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE aotransform_test

// Third party includes
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/aotransform.h"
#include "votca/xtp/orbitals.h"
#include <votca/tools/eigenio_matrixmarket.h>

using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(aotransform_test)

BOOST_AUTO_TEST_CASE(transform_test) {
  QMAtom a(0, "C", Eigen::Vector3d::Zero());
  QMMolecule mol("zero", 0);
  mol.push_back(a);

  BasisSet bs;
  bs.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aotransform/all.xml");
  AOBasis basis;
  basis.Fill(bs, mol);

  std::array<Eigen::MatrixXd, 7> ref;

  for (unsigned i = 0; i < ref.size(); i++) {
    ref[i] = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/aotransform/ref_" +
        std::to_string(i) + ".mm");
  }

  Index ref_index = 0;
  for (const AOShell& shell : basis) {
    for (const AOGaussianPrimitive& gauss : shell) {
      Eigen::MatrixXd transform = AOTransform::getTrafo(gauss);
      bool check_transform = ref[ref_index].isApprox(transform, 1e-5);
      BOOST_CHECK_EQUAL(check_transform, 1);
      if (!check_transform) {
        std::cout << "ref " << xtp::EnumToString(shell.getL()) << std::endl;
        std::cout << ref[ref_index] << std::endl;
        std::cout << "result" << std::endl;
        std::cout << transform << std::endl;
      }
      ref_index++;
    }
  }
}

BOOST_AUTO_TEST_CASE(xintegrate) {

  BOOST_REQUIRE_THROW(AOTransform::XIntegrate(0, 0.1), std::runtime_error);

  BOOST_REQUIRE_THROW(AOTransform::XIntegrate(1, -0.1);, std::runtime_error);

  Eigen::VectorXd res1 = AOTransform::XIntegrate(1, 0.1);
  Eigen::VectorXd res1_ref = Eigen::VectorXd::Zero(1);
  res1_ref << 0.967643;
  bool check_res1 = res1.isApprox(res1_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res1, 1);
  if (!check_res1) {
    std::cout << "ref" << std::endl;
    std::cout << res1_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res1 << std::endl;
  }
  Eigen::VectorXd res2 = AOTransform::XIntegrate(5, 0.1);
  Eigen::VectorXd res2_ref = Eigen::VectorXd::Zero(5);
  res2_ref << 0.967643, 0.314029, 0.186255, 0.132188, 0.102394;

  bool check_res2 = res2.isApprox(res2_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res2, 1);
  if (!check_res1) {
    std::cout << "ref" << std::endl;
    std::cout << res2_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res2 << std::endl;
  }

  Eigen::VectorXd res3 = AOTransform::XIntegrate(1, 1e-12);
  Eigen::VectorXd res3_ref = Eigen::VectorXd::Zero(1);
  res3_ref[0] = 1.0;
  bool check_res3 = res3.isApprox(res3_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res3, 1);
  if (!check_res3) {
    std::cout << "ref" << std::endl;
    std::cout << res3_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res3 << std::endl;
  }

  Eigen::VectorXd res4 = AOTransform::XIntegrate(5, 1e-12);
  Eigen::VectorXd res4_ref = Eigen::VectorXd::Zero(5);
  res4_ref << 1, 0.333333, 0.2, 0.142857, 0.111111;

  bool check_res4 = res4.isApprox(res4_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res4, 1);
  if (!check_res4) {
    std::cout << "ref" << std::endl;
    std::cout << res4_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res4 << std::endl;
  }

  Eigen::VectorXd res5 = AOTransform::XIntegrate(1, 15);
  Eigen::VectorXd res5_ref = Eigen::VectorXd::Zero(1);
  res5_ref << 0.228823;
  bool check_res5 = res5.isApprox(res5_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res5, 1);
  if (!check_res5) {
    std::cout << "ref" << std::endl;
    std::cout << res5_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res5 << std::endl;
  }

  Eigen::VectorXd res6 = AOTransform::XIntegrate(5, 15);
  Eigen::VectorXd res6_ref = Eigen::VectorXd::Zero(5);
  res6_ref << 0.228823, 0.00762742, 0.000762731, 0.000127112, 2.96492e-05;
  bool check_res6 = res6.isApprox(res6_ref, 1e-5);
  BOOST_CHECK_EQUAL(check_res6, 1);
  if (!check_res6) {
    std::cout << "ref" << std::endl;
    std::cout << res6_ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << res6 << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(blocksize) {
  BOOST_CHECK_EQUAL(AOTransform::getBlockSize(0), 1);

  BOOST_CHECK_EQUAL(AOTransform::getBlockSize(0), 1);
  BOOST_CHECK_EQUAL(AOTransform::getBlockSize(1), 4);
  BOOST_CHECK_EQUAL(AOTransform::getBlockSize(2), 10);
  BOOST_CHECK_EQUAL(AOTransform::getBlockSize(3), 20);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
using namespace std;

void MiniMax(long long arr[])
{
int i;
int j=0;
int128_t  MaxSum= INT_MIN;
int128_t MinSum=INT_MAX;
for (i=0; i<5 ;i++)
{
    int128_t sum=0;
    for(j=0; j<5; j++)
    {
        if(j != i)
        {
            sum +=arr[j];
        }
    }
    MaxSum= max(sum, MaxSum);
    MinSum=min(MinSum, sum);
}

cout<<MinSum<<" "<<MaxSum<<endl;
}

int main()
{
    int128_t arr[5];
    for(int i=0;i<5;i++)
    {
      cin>>arr[i];
    }
    MiniMax(arr);
    return 0;
}