/*******************************************************************************
 *
 * Standard domain of intervals.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Contributors: Alexandre C. D. Wimmers (alexandre.c.wimmers@nasa.gov)
 *
 * The resolution of a system of linear constraints over the domain of intervals
 * is based on W. Harvey & P. J. Stuckey's paper: Improving linear constraint
 * propagation by changing constraint representation, in Constraints,
 * 8(2):173–207, 2003.
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#ifndef IKOS_INTERVALS_HPP
#define IKOS_INTERVALS_HPP

#include <vector>
#include <set>
#include <map>
#include <boost/optional.hpp>
#include <crab/common/types.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/bignums.hpp>
#include <crab/domains/linear_constraints.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/domains/operators_api.hpp>

namespace ikos {

  template< typename Number >
  class bound;

  template< typename Number >
  class bound: public writeable {

    template < typename Any > friend class bound;

  public:
    typedef bound< Number > bound_t;

  private:
    bool _is_infinite;
    Number _n;

  private:
    bound();

    bound(bool is_infinite, Number n): _is_infinite(is_infinite), _n(n) {
      if (is_infinite){
        if (n > 0)
          this->_n = 1;
        else
          this->_n = -1;
      }
    }

  public:
    static bound_t min(bound_t x, bound_t y) {
      return (x.operator<=(y) ? x : y);
    }

    static bound_t min(bound_t x, bound_t y, bound_t z) {
      return min(x, min(y, z));
    }

    static bound_t min(bound_t x, bound_t y, bound_t z, bound_t t) {
      return min(x, min(y, z, t));
    }

    static bound_t max(bound_t x, bound_t y) {
      return (x.operator<=(y) ? y : x);
    }

    static bound_t max(bound_t x, bound_t y, bound_t z) {
      return max(x, max(y, z));
    }

    static bound_t max(bound_t x, bound_t y, bound_t z, bound_t t) {
      return max(x, max(y, z, t));
    }

    static bound_t plus_infinity() {
      return bound_t(true, 1);
    }

    static bound_t minus_infinity() {
      return bound_t(true, -1);
    }

  public:
    bound(int n): _is_infinite(false), _n(n) { }

    bound(std::string s): _n(1) {
      if (s == "+oo") {
        this->_is_infinite = true;
      } else if (s == "-oo") {
        this->_is_infinite = true;
        this->_n = -1;
      } else {
        this->_is_infinite = false;
        this->_n = Number(s);
      }
    }

    bound(Number n): _is_infinite(false), _n(n) { }

    bound(const bound_t& o): writeable(), _is_infinite(o._is_infinite), _n(o._n) { }

    bound_t& operator=(const bound_t &o){
      if (this != &o) {
        this->_is_infinite = o._is_infinite;
        this->_n = o._n;
      }
      return *this;
    }

    bool is_infinite() const {
      return this->_is_infinite;
    }

    bool is_finite() const {
      return !this->_is_infinite;
    }

    bool is_plus_infinity() const {
      return (this->is_infinite() && this->_n > 0);
    }

    bool is_minus_infinity() const {
      return (this->is_infinite() && this->_n < 0);
    }

    bound_t operator-() const {
      return bound_t(this->_is_infinite, -this->_n);
    }

    bound_t operator+(bound_t x) const {
      if (this->is_finite() && x.is_finite()) {
        return bound_t(this->_n + x._n);
      } else if (this->is_finite() && x.is_infinite()) {
        return x;
      } else if (this->is_infinite() && x.is_finite()) {
        return *this;
      } else if (this->_n == x._n) {
        return *this;
      } else {
        CRAB_ERROR("Bound: undefined operation -oo + +oo");
      }
    }

    bound_t& operator+=(bound_t x)  {
      return this->operator=(this->operator+(x));
    }

    bound_t operator-(bound_t x) const {
      return this->operator+(x.operator-());
    }

    bound_t& operator-=(bound_t x)  {
      return this->operator=(this->operator-(x));
    }

    bound_t operator*(bound_t x) const {
      if (x._n == 0)
        return x;
      else if (this->_n == 0)
        return *this;
      else
        return bound_t(this->_is_infinite || x._is_infinite, this->_n * x._n);
    }

    bound_t& operator*=(bound_t x)  {
      return this->operator=(this->operator*(x));
    }

    bound_t operator/(bound_t x) const {
      if (x._n == 0) {
        CRAB_ERROR("Bound: division by zero");
      } else if (this->is_finite() && x.is_finite()) {
        return bound_t(false, _n / x._n);
      } else if (this->is_finite() && x.is_infinite()) {
        if (this->_n > 0) {
          return x;
        } else if (this->_n == 0) {
          return *this;
        } else {
          return x.operator-();
        }
      } else if (this->is_infinite() && x.is_finite()) {
        if (x._n > 0) {
          return *this;
        } else {
          return this->operator-();
        }
      } else {
        return bound_t(true, this->_n * x._n);
      }
    }

    bound_t& operator/=(bound_t x) {
      return this->operator=(this->operator/(x));
    }

    bool operator<(bound_t x) const {
      return !this->operator>=(x);
    }

    bool operator>(bound_t x) const {
      return !this->operator<=(x);
    }

    bool operator==(bound_t x) const {
      return (this->_is_infinite == x._is_infinite && this->_n == x._n);
    }

    bool operator!=(bound_t x) const {
      return !this->operator==(x);
    }

    /*	operator<= and operator>= use a somewhat optimized implementation.
     *	results include up to 20% improvements in performance in the octagon domain
     *	over a more naive implementation.
     */
    bool operator<=(bound_t x) const {
      if(this->_is_infinite xor x._is_infinite){
        if(this->_is_infinite){
          return this->_n < 0;
        }
        return x._n > 0;
      }
      return this->_n <= x._n;
    }

    bool operator>=(bound_t x) const {
      if(this->_is_infinite xor x._is_infinite){
        if(this->_is_infinite){
          return this->_n > 0;
        }
        return x._n < 0;
      }
      return this->_n >= x._n;
    }

    bound_t abs() const {
      if (this->operator>=(0)) {
        return *this;
      } else {
        return this->operator-();
      }
    }

    boost::optional< Number > number() const {
      if (this->is_infinite()) {
        return boost::optional< Number >();
      } else {
        return boost::optional< Number >(this->_n);
      }
    }

    void write(crab::crab_os& o) {
      if (this->is_plus_infinity()) {
        o << "+oo";
      } else if (this->is_minus_infinity()) {
        o << "-oo";
      } else {
        o << this->_n;
      }
    }

  }; // class bound

  typedef bound< z_number > z_bound;
  typedef bound< q_number > q_bound;

  template< typename Number >
  class interval;

  template< typename Number >
  class interval: public writeable {

  public:
    typedef bound< Number > bound_t;
    typedef interval< Number > interval_t;

  private:
    bound_t _lb;
    bound_t _ub;

  public:
    static interval_t top() {
      return interval_t(bound_t::minus_infinity(), bound_t::plus_infinity());
    }

    static interval_t bottom() {
      return interval_t();
    }

  private:
    interval(): _lb(0), _ub(-1) { }

    static Number abs(Number x) { return x < 0 ? -x : x; }

    static Number max(Number x, Number y) { return x.operator<=(y) ? y : x; }

    static Number min(Number x, Number y) { return x.operator<(y) ? x : y; }

  public:
    interval(bound_t lb, bound_t ub): _lb(lb), _ub(ub) {
      if (lb > ub) {
        this->_lb = 0;
        this->_ub = -1;
      }
    }

    interval(bound_t b): _lb(b), _ub(b) {
      if (b.is_infinite()) {
        this->_lb = 0;
        this->_ub = -1;
      }
    }

    interval(Number n): _lb(n), _ub(n) { }

    interval(std::string b): _lb(b), _ub(b) {
      if (this->_lb.is_infinite()) {
        this->_lb = 0;
        this->_ub = -1;
      }
    }

    interval(const interval_t& i): writeable(), _lb(i._lb), _ub(i._ub) { }

    interval_t& operator=(interval_t i){
      this->_lb = i._lb;
      this->_ub = i._ub;
      return *this;
    }

    bound_t lb() const {
      return this->_lb;
    }

    bound_t ub() const {
      return this->_ub;
    }

    bool is_bottom() const {
      return (this->_lb > this->_ub);
    }

    bool is_top() const {
      return (this->_lb.is_infinite() && this->_ub.is_infinite());
    }

    interval_t lower_half_line() const {
      return interval_t(bound_t::minus_infinity(), this->_ub);
    }

    interval_t upper_half_line() const {
      return interval_t(this->_lb, bound_t::plus_infinity());
    }

    bool operator==(interval_t x) const {
      if (is_bottom()) {
        return x.is_bottom();
      } else {
        return (this->_lb == x._lb) && (this->_ub == x._ub);
      }
    }

    bool operator!=(interval_t x) const {
      return !this->operator==(x);
    }

    bool operator<=(interval_t x) const {
      if (this->is_bottom()) {
        return true;
      } else if (x.is_bottom()) {
        return false;
      } else {
        return (x._lb <= this->_lb) && (this->_ub <= x._ub);
      }
    }

    interval_t operator|(interval_t x) const {
      if (this->is_bottom()) {
        return x;
      } else if (x.is_bottom()) {
        return *this;
      } else {
	return interval_t(bound_t::min(this->_lb, x._lb),
                            bound_t::max(this->_ub, x._ub));
      }
    }

    interval_t operator&(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return interval_t(bound_t::max(this->_lb, x._lb),
                          bound_t::min(this->_ub, x._ub));
      }
    }

    interval_t operator||(interval_t x) const {
      if (this->is_bottom()) {
	return x;
      } else if (x.is_bottom()) {
	return *this;
      } else {
        return interval_t(x._lb < this->_lb ?
                          bound_t::minus_infinity() :
                          this->_lb,
                          this->_ub < x._ub ?
                          bound_t::plus_infinity() :
                          this->_ub);
      }
    }

    template<typename Thresholds>
    interval_t widening_thresholds (interval_t x, const Thresholds &ts) {
      if (this->is_bottom()) {
	return x;
      } else if (x.is_bottom()) {
	return *this;
      } else {
        bound_t lb = (x._lb < this->_lb ? ts.get_prev (x._lb) : this->_lb);
        bound_t ub = (this->_ub < x._ub ? ts.get_next (x._ub) :  this->_ub);
        return interval_t(lb, ub);
      }
    }

    interval_t operator&&(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return interval_t(this->_lb.is_infinite() && x._lb.is_finite() ?
                          x._lb : this->_lb,
                          this->_ub.is_infinite() && x._ub.is_finite() ?
                          x._ub : this->_ub);
      }
    }

    interval_t operator+(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
	return interval_t(this->_lb + x._lb, this->_ub + x._ub);
      }
    }

    interval_t& operator+=(interval_t x) {
      return this->operator=(this->operator+(x));
    }

    interval_t operator-() const {
      if (this->is_bottom()) {
        return this->bottom();
      } else {
        return interval_t(-this->_ub, -this->_lb);
      }
    }

    interval_t operator-(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return interval_t(this->_lb - x._ub, this->_ub - x._lb);
      }
    }

    interval_t& operator-=(interval_t x) {
      return this->operator=(this->operator-(x));
    }

    interval_t operator*(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        bound_t ll = this->_lb * x._lb;
        bound_t lu = this->_lb * x._ub;
        bound_t ul = this->_ub * x._lb;
        bound_t uu = this->_ub * x._ub;
        return interval_t(bound_t::min(ll, lu, ul, uu),
                          bound_t::max(ll, lu, ul, uu));
      }
    }

    interval_t& operator*=(interval_t x) {
      return this->operator=(this->operator*(x));
    }

    interval_t operator/(interval_t x) const;

    interval_t& operator/=(interval_t x) {
      return this->operator=(this->operator/(x));
    }

    boost::optional< Number > singleton() const {
      if (!this->is_bottom() && this->_lb == this->_ub) {
        return this->_lb.number();
      } else {
        return boost::optional< Number >();
      }
    }

    bool operator[](Number n) const {
      if (this->is_bottom()) {
        return false;
      } else {
        bound_t b(n);
        return (this->_lb <= b) && (b <= this->_ub);
      }
    }

    void write(crab::crab_os& o) {
      if (is_bottom()) {
        o << "_|_";
      } else {
        o << "[" << _lb << ", " << _ub << "]";
      }
    }

    // division and remainder operations

    interval_t UDiv(interval_t x ) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t SRem(interval_t x)  const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t URem(interval_t x)  const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    // bitwise operations

    interval_t Trunc(unsigned /* width */) const {
      return *this;
    }

    interval_t ZExt(unsigned /* width */) const {
      return *this;
    }

    interval_t SExt(unsigned /* width */) const {
      return *this;
    }

    interval_t And(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t Or(interval_t x)  const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t Xor(interval_t x) const { return this->Or(x); }

    interval_t Shl(interval_t x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t LShr(interval_t  x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

    interval_t AShr(interval_t  x) const {
      if (this->is_bottom() || x.is_bottom()) {
        return this->bottom();
      } else {
        return this->top();
      }
    }

  };//  class interval

  template<>
  inline interval< q_number > interval< q_number >::
  operator/(interval< q_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< q_number > d = x.singleton();
      if (d && *d == 0) {
        // [_, _] / 0 = _|_
        return this->bottom();
      } else if (x[0]) {
        boost::optional< q_number > n = this->singleton();
        if (n && *n == 0) {
          // 0 / [_, _] = 0
          return interval_t(q_number(0));
        } else {
          return this->top();
        }
      } else {
        bound_t ll = this->_lb / x._lb;
        bound_t lu = this->_lb / x._ub;
        bound_t ul = this->_ub / x._lb;
        bound_t uu = this->_ub / x._ub;
        return interval_t(bound_t::min(ll, lu, ul, uu),
                          bound_t::max(ll, lu, ul, uu));
      }
    }
  }

  template<>
  inline interval< z_number > interval< z_number >::
  operator/(interval< z_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      typedef interval< z_number > z_interval;
      if (x[0]) {
        z_interval l(x._lb, z_bound(-1));
        z_interval u(z_bound(1), x._ub);
        return (this->operator/(l) | this->operator/(u));
      } else if (this->operator[](0)) {
        z_interval l(this->_lb, z_bound(-1));
        z_interval u(z_bound(1), this->_ub);
        return ((l / x) | (u / x) | z_interval(z_number(0)));
      } else {
        // Neither the dividend nor the divisor contains 0
        z_interval a = (this->_ub < 0) ?
            (*this + ((x._ub < 0) ?
                      (x + z_interval(z_number(1))) :
                      (z_interval(z_number(1)) - x))) : *this;
	bound_t ll = a._lb / x._lb;
	bound_t lu = a._lb / x._ub;
	bound_t ul = a._ub / x._lb;
	bound_t uu = a._ub / x._ub;
	return interval_t(bound_t::min(ll, lu, ul, uu),
                            bound_t::max(ll, lu, ul, uu));
      }
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  SRem(interval< z_number > x) const {
    // note that the sign of the divisor does not matter
    typedef interval< z_number > z_interval;

    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else if (this->singleton() && x.singleton()) {
      z_number dividend = *this->singleton();
      z_number divisor = *x.singleton();

      if (divisor == 0) {
        return this->bottom();
      }

      return interval_t(dividend % divisor);
    } else if (x.ub().is_finite() && x.lb().is_finite()) {
      z_number max_divisor = max(abs(*x.lb().number()),
                                 abs(*x.ub().number()));

      if (max_divisor == 0) {
        return this->bottom();
      }

      if (this->lb() < 0) {
        if (this->ub() > 0) {
          return interval_t(-(max_divisor - 1), max_divisor - 1);
        } else {
          return interval_t(-(max_divisor - 1), 0);
        }
      } else {
        return interval_t(0, max_divisor - 1);
      }
    } else {
      return this->top();
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  URem(interval< z_number > x) const {
    typedef interval< z_number > z_interval;

    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else if (this->singleton() && x.singleton()) {
      z_number dividend = *this->singleton();
      z_number divisor = *x.singleton();

      if (divisor < 0) {
        return this->top();
      } else if (divisor == 0) {
        return this->bottom();
      } else if (dividend < 0) {
        // dividend is treated as an unsigned integer.
        // we would need the size to be more precise
        return interval_t(0, divisor - 1);
      } else {
        return interval_t(dividend % divisor);
      }
    } else if (x.ub().is_finite() && x.lb().is_finite()) {
      z_number max_divisor = *x.ub().number();

      if (x.lb() < 0 || x.ub() < 0) {
        return this->top();
      } else if (max_divisor == 0) {
        return this->bottom();
      }

      return interval_t(0, max_divisor - 1);
    } else {
      return this->top();
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  And(interval< z_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< z_number > left_op = this->singleton();
      boost::optional< z_number > right_op = x.singleton();

      if (left_op && right_op) {
        return interval_t((*left_op) & (*right_op));
      } else if (this->lb() >= 0 && x.lb() >= 0) {
        return interval_t(0, bound_t::min(this->ub(), x.ub()));
      } else {
        return this->top();
      }
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  Or(interval< z_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< z_number > left_op = this->singleton();
      boost::optional< z_number > right_op = x.singleton();

      if (left_op && right_op) {
        return interval_t((*left_op) | (*right_op));
      } else if (this->lb() >= 0 && x.lb() >= 0) {
        boost::optional< z_number > left_ub = this->ub().number();
        boost::optional< z_number > right_ub = x.ub().number();

        if (left_ub && right_ub) {
          z_number m = (*left_ub > *right_ub ? *left_ub : *right_ub);
          return interval_t(0, m.fill_ones());
        } else {
          return interval_t(0, bound_t::plus_infinity());
        }
      } else {
        return this->top();
      }
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  Xor(interval< z_number > x)  const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< z_number > left_op = this->singleton();
      boost::optional< z_number > right_op = x.singleton();

      if (left_op && right_op) {
        return interval_t((*left_op) ^ (*right_op));
      } else {
        return this->Or(x);
      }
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  Shl(interval< z_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< z_number > shift = x.singleton();
      if (shift) {
        z_number factor = 1;
        for (int i = 0; (*shift) > i; i++) {
          factor *= 2;
        }
        return (*this) * factor;
      } else {
        return this->top();
      }
    }
  }

  template <>
  inline interval< z_number > interval< z_number >::
  LShr(interval< z_number > x) const {
    if (this->is_bottom() || x.is_bottom()) {
      return this->bottom();
    } else {
      boost::optional< z_number > shift = x.singleton();

      if (this->lb() >= 0 && this->ub().is_finite() && shift) {
        z_number lb = *this->lb().number();
        z_number ub = *this->ub().number();
        return interval< z_number >(lb >> *shift, ub >> *shift);
      } else {
        return this->top();
      }
    }
  }

  template< typename Number >
  inline interval< Number > operator+(Number c, interval< Number > x) {
    return interval< Number >(c) + x;
  }

  template< typename Number >
  inline interval< Number > operator+(interval< Number > x, Number c) {
    return x + interval< Number >(c);
  }

  template< typename Number >
  inline interval< Number > operator*(Number c, interval< Number > x) {
    return interval< Number >(c) * x;
  }

  template< typename Number >
  inline interval< Number > operator*(interval< Number > x, Number c) {
    return x * interval< Number >(c);
  }

  template< typename Number >
  inline interval< Number > operator/(Number c, interval< Number > x) {
    return interval< Number >(c) / x;
  }

  template< typename Number >
  inline interval< Number > operator/(interval< Number > x, Number c) {
    return x / interval< Number >(c);
  }

  template< typename Number >
  inline interval< Number > operator-(Number c, interval< Number > x) {
    return interval< Number >(c) - x;
  }

  template< typename Number >
  inline interval< Number > operator-(interval< Number > x, Number c) {
    return x - interval< Number >(c);
  }

  template < typename Number >
  inline crab::crab_os& operator<<(crab::crab_os& o, interval< Number > i) {
    i.write(o);
    return o;
  }

  typedef interval< z_number > z_interval;
  typedef interval< q_number > q_interval;

  namespace intervals_impl {

    template< typename Interval, typename Number >
    inline Interval trim_bound(Interval i, Number c);

    template<>
    inline z_interval trim_bound(z_interval i, z_number c) {
      if (i.lb() == c) {
	return z_interval(c + 1, i.ub());
      } else if (i.ub() == c) {
	return z_interval(i.lb(), c - 1);
      } else {
	return i;
      }
    }

    template<>
    inline q_interval trim_bound(q_interval i, q_number /* c */) {
      // No refinement possible for disequations over rational numbers
      return i;
    }

  } // namespace intervals_impl

  template< typename Number, typename VariableName, typename IntervalCollection >
  class linear_interval_solver {

  public:
    typedef typename IntervalCollection::value_type Interval;
    typedef variable< Number, VariableName > variable_t;
    typedef linear_expression< Number, VariableName > linear_expression_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef linear_constraint_system< Number, VariableName > linear_constraint_system_t;

  private:
    typedef std::vector< linear_constraint_t > cst_table_t;
    typedef std::set< unsigned int > uint_set_t;
    typedef std::map< variable_t, uint_set_t > trigger_table_t;
    typedef typename linear_constraint_t::variable_set_t variable_set_t;

  private:
    class bottom_found { };

  private:
    std::size_t _max_cycles;
    std::size_t _max_op;
    bool _is_contradiction;
    bool _is_large_system;
    cst_table_t _cst_table;
    trigger_table_t _trigger_table;
    variable_set_t _refined_variables;
    std::size_t _op_count;

  private:
    static const std::size_t _large_system_cst_threshold = 3;
    // cost of one propagation cycle for a dense 3x3 system of constraints
    static const std::size_t _large_system_op_threshold = 27;

  private:
    void refine(variable_t v, Interval i, IntervalCollection& env) {
      VariableName n = v.name();
      Interval old_i = env[n];
      Interval new_i = old_i & i;
      if (new_i.is_bottom()) {
	throw bottom_found();
      }
      if (!(old_i == new_i)) {
	env.set(n, new_i);
	this->_refined_variables += v;
	++(this->_op_count);
      }
    }

    Interval compute_residual(linear_constraint_t cst, variable_t pivot,
                              IntervalCollection& env) {
      Interval residual(cst.constant());
      for (typename linear_constraint_t::iterator it = cst.begin();
           it != cst.end(); ++it) {
	variable_t v = it->second;
	if (v.index() != pivot.index()) {
	  residual = residual - (Interval (it->first) * env[v.name()]);
	  ++(this->_op_count);
	}
      }
      return residual;
    }

    void propagate(linear_constraint_t cst, IntervalCollection& env) {
      for (typename linear_constraint_t::iterator it = cst.begin();
           it != cst.end(); ++it) {
	Number c = it->first;
	variable_t pivot = it->second;
	Interval rhs = this->compute_residual(cst, pivot, env) / Interval(c);
	if (cst.is_equality()) {
	  this->refine(pivot, rhs, env);
	} else if (cst.is_inequality()) {
	  if (c > 0) {
	    refine(pivot, rhs.lower_half_line(), env);
	  } else {
	    refine(pivot, rhs.upper_half_line(), env);
	  }
	} else {
	  // cst is a disequation
	  boost::optional< Number > c = rhs.singleton();
	  if (c) {
	    Interval old_i = env[pivot.name()];
              Interval new_i = intervals_impl::trim_bound (old_i, *c);
	    if (new_i.is_bottom()) {
	      throw bottom_found();
	    }
	    if (!(old_i == new_i)) {
	      env.set(pivot.name(), new_i);
	      this->_refined_variables += pivot;
	    }
	    ++(this->_op_count);
	  }
	}
      }
    }

    void solve_large_system(IntervalCollection& env) {
      this->_op_count = 0;
      this->_refined_variables.clear();
      for (typename cst_table_t::iterator it = this->_cst_table.begin();
           it != this->_cst_table.end(); ++it) {
	this->propagate(*it, env);
      }
      do {
	variable_set_t vars_to_process(this->_refined_variables);
	this->_refined_variables.clear();
	for (typename variable_set_t::iterator it = vars_to_process.begin();
               it != vars_to_process.end(); ++it) {
	  uint_set_t& csts = this->_trigger_table[*it];
	  for (typename uint_set_t::iterator cst_it = csts.begin();
                 cst_it != csts.end(); ++cst_it) {
	    this->propagate(this->_cst_table.at(*cst_it), env);
	  }
	}
      }
      while (!this->_refined_variables.empty() &&
             this->_op_count <= this->_max_op);
    }

    void solve_small_system(IntervalCollection& env) {
      std::size_t cycle = 0;
      do {
	++cycle;
	this->_refined_variables.clear();
	for (typename cst_table_t::iterator it = this->_cst_table.begin();
               it != this->_cst_table.end(); ++it) {
	  this->propagate(*it, env);
	}
      }
      while (!this->_refined_variables.empty() &&
             cycle <= this->_max_cycles);
    }

  public:

    linear_interval_solver(linear_constraint_system_t csts,
                           std::size_t max_cycles):
        _max_cycles(max_cycles),
        _is_contradiction(false),
        _is_large_system(false),
        _op_count(0) {

      std::size_t op_per_cycle = 0;
      for (typename linear_constraint_system_t::iterator it = csts.begin();
           it != csts.end(); ++it) {
	linear_constraint_t cst = *it;
	if (cst.is_contradiction()) {
	  this->_is_contradiction = true;
	  return;
	} else if (cst.is_tautology()) {
	  continue;
	} else {
	  std::size_t cst_size = cst.size();
	  this->_cst_table.push_back(cst);
            // cost of one reduction step on the constraint in terms
            // of accesses to the interval collection
	  op_per_cycle += cst_size * cst_size;
	}
      }

      this->_is_large_system = (this->_cst_table.size() >
                                _large_system_cst_threshold) ||
          (op_per_cycle > _large_system_op_threshold);

      if (!this->_is_contradiction && this->_is_large_system) {
	this->_max_op = op_per_cycle * max_cycles;
	for (unsigned int i = 0; i < this->_cst_table.size(); ++i) {
	  linear_constraint_t cst = this->_cst_table.at(i);
	  variable_set_t vars = cst.variables();
	  for (typename variable_set_t::iterator it = vars.begin();
                 it != vars.end(); ++it) {
	    this->_trigger_table[*it].insert(i);
	  }
	}
      }
    }

    void run(IntervalCollection& env) {
      if (this->_is_contradiction) {
        env.set_to_bottom();
      } else {
        try {
          if (this->_is_large_system) {
	    this->solve_large_system(env);
          } else {
            this->solve_small_system(env);
          }
        }
        catch (bottom_found& e) {
          env.set_to_bottom();
        }
      }
    }

  }; // class linear_interval_solver

  template< typename Number, typename VariableName, std::size_t max_reduction_cycles = 10 >
  class interval_domain: public writeable,
                         public numerical_domain< Number, VariableName >,
                         public bitwise_operators< Number, VariableName >,
                         public division_operators< Number, VariableName >,
                         public crab::domains::array_operators< Number, VariableName >,
                         public crab::domains::pointer_operators< Number, VariableName > {

  public:
    using typename numerical_domain< Number, VariableName >::linear_expression_t;
    using typename numerical_domain< Number, VariableName >::linear_constraint_t;
    using typename numerical_domain< Number, VariableName >::linear_constraint_system_t;
    using typename numerical_domain< Number, VariableName >::variable_t;
    using typename numerical_domain< Number, VariableName >::number_t;
    using typename numerical_domain< Number, VariableName >::varname_t;
    typedef interval< Number > interval_t;
    typedef interval_domain< Number, VariableName > interval_domain_t;

  private:
    typedef separate_domain< VariableName, interval_t > separate_domain_t;
    typedef linear_interval_solver< Number, VariableName, separate_domain_t > solver_t;

  public:
    typedef typename separate_domain_t::iterator iterator;

  private:
    separate_domain_t _env;

  private:
    interval_domain(separate_domain_t env): _env(env) { }

  public:
    static interval_domain_t top() {
      return interval_domain(separate_domain_t::top());
    }

    static interval_domain_t bottom() {
      return interval_domain(separate_domain_t::bottom());
    }

  public:
    interval_domain(): _env(separate_domain_t::top()) { }

    interval_domain(const interval_domain_t& e):
      writeable(),
      numerical_domain< Number, VariableName >(),
      bitwise_operators< Number, VariableName >(),
      division_operators< Number, VariableName >(),
      crab::domains::array_operators< Number, VariableName > (),
      crab::domains::pointer_operators< Number, VariableName > (),
      _env(e._env) {
      crab::CrabStats::count (getDomainName() + ".count.copy");
      crab::ScopedCrabStats __st__(getDomainName() + ".copy");
    }

    interval_domain_t& operator=(const interval_domain_t& o) {
      crab::CrabStats::count (getDomainName() + ".count.copy");
      crab::ScopedCrabStats __st__(getDomainName() + ".copy");
      if (this != &o)
        this->_env = o._env;
      return *this;
    }

    iterator begin() {
      return this->_env.begin();
    }

    iterator end() {
      return this->_env.end();
    }

    bool is_bottom() {
      return this->_env.is_bottom();
    }

    bool is_top() {
      return this->_env.is_top();
    }

    bool operator<=(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.leq");
      crab::ScopedCrabStats __st__(getDomainName() + ".leq");
      return (this->_env <= e._env);
    }

    void operator|=(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.join");
      crab::ScopedCrabStats __st__(getDomainName() + ".join");
      this->_env = this->_env | e._env;
    }

    interval_domain_t operator|(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.join");
      crab::ScopedCrabStats __st__(getDomainName() + ".join");
      return (this->_env | e._env);
    }

    interval_domain_t operator&(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.meet");
      crab::ScopedCrabStats __st__(getDomainName() + ".meet");
      return (this->_env & e._env);
    }

    interval_domain_t operator||(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.widening");
      crab::ScopedCrabStats __st__(getDomainName() + ".widening");
      return (this->_env || e._env);
    }

    template<typename Thresholds>
    interval_domain_t widening_thresholds (interval_domain_t e, const Thresholds &ts) {
      crab::CrabStats::count (getDomainName() + ".count.widening");
      crab::ScopedCrabStats __st__(getDomainName() + ".widening");
      return this->_env.widening_thresholds (e._env, ts);
    }

    interval_domain_t operator&&(interval_domain_t e) {
      crab::CrabStats::count (getDomainName() + ".count.narrowing");
      crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");
      return (this->_env && e._env);
    }

    void set(VariableName v, interval_t i) {
      crab::CrabStats::count (getDomainName() + ".count.assign");
      crab::ScopedCrabStats __st__(getDomainName() + ".assign");
      this->_env.set(v, i);
    }

    void set(VariableName v, Number n) {
      crab::CrabStats::count (getDomainName() + ".count.assign");
      crab::ScopedCrabStats __st__(getDomainName() + ".assign");
      this->_env.set(v, interval_t(n));
    }

    void operator-=(VariableName v) {
      crab::CrabStats::count (getDomainName() + ".count.forget");
      crab::ScopedCrabStats __st__(getDomainName() + ".forget");
      this->_env -= v;
    }

    interval_t operator[](VariableName v) {
      return this->_env[v];
    }

    interval_t operator[](linear_expression_t expr) {
      interval_t r(expr.constant());
      for (typename linear_expression_t::iterator it = expr.begin();
           it != expr.end(); ++it) {
	interval_t c(it->first);
	r += c * this->_env[it->second.name()];
      }
      return r;
    }

    void operator+=(linear_constraint_system_t csts) {
      crab::CrabStats::count (getDomainName() + ".count.add_constraints");
      crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");
      this->add(csts);
    }

    void add(linear_constraint_system_t csts,
             std::size_t threshold = max_reduction_cycles) {
      if (!this->is_bottom()) {
	solver_t solver(csts, threshold);
	solver.run(this->_env);
      }
    }

    interval_domain_t operator+(linear_constraint_system_t csts) {
      interval_domain_t e(this->_env);
      e += csts;
      return e;
    }

    void assign(VariableName x, linear_expression_t e) {
      crab::CrabStats::count (getDomainName() + ".count.assign");
      crab::ScopedCrabStats __st__(getDomainName() + ".assign");

      if (boost::optional<variable_t> v = e.get_variable ()) {
        this->_env.set(x, this->_env [(*v).name ()]);
      }
      else {
        interval_t r = e.constant();
        for (typename linear_expression_t::iterator it = e.begin();
             it != e.end(); ++it) {
	r += it->first * this->_env[it->second.name()];
        }
        this->_env.set(x, r);
      }
    }

    void apply(operation_t op, VariableName x, VariableName y, VariableName z) {
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi = this->_env[z];
      interval_t xi = interval_t::bottom();

      switch (op) {
      case OP_ADDITION: {
	xi = yi + zi;
	break;
      }
      case OP_SUBTRACTION: {
	xi = yi - zi;
	break;
      }
      case OP_MULTIPLICATION: {
	xi = yi * zi;
	break;
      }
      case OP_DIVISION: {
	xi = yi / zi;
	break;
      }
      }
      this->_env.set(x, xi);
    }

    void apply(operation_t op, VariableName x, VariableName y, Number k) {
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi(k);
      interval_t xi = interval_t::bottom();

      switch (op) {
      case OP_ADDITION: {
	xi = yi + zi;
	break;
      }
      case OP_SUBTRACTION: {
	xi = yi - zi;
	break;
      }
      case OP_MULTIPLICATION: {
	xi = yi * zi;
	break;
      }
      case OP_DIVISION: {
	xi = yi / zi;
	break;
      }
      }
      this->_env.set(x, xi);
    }

    // bitwise_operators_api

    void apply(conv_operation_t op, VariableName x, VariableName y, unsigned width){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t xi = interval_t::bottom();
      switch(op){
        case OP_TRUNC:
          xi = yi.Trunc(width);
          break;
        case OP_ZEXT:
          xi = yi.ZExt(width);
          break;
        case OP_SEXT:
          xi = yi.SExt(width);
          break;
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);
    }

    void apply(conv_operation_t op, VariableName x, Number k, unsigned width){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi(k);
      interval_t xi = interval_t::bottom();
      switch(op){
        case OP_TRUNC:
          xi = yi.Trunc(width);
          break;
        case OP_ZEXT:
          xi = yi.ZExt(width);
          break;
        case OP_SEXT:
          xi = yi.SExt(width);
          break;
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);
    }

    void apply(bitwise_operation_t op, VariableName x, VariableName y, VariableName z){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi = this->_env[z];
      interval_t xi = interval_t::bottom();

      switch (op) {
        case OP_AND: {
	xi = yi.And(zi);
	break;
        }
        case OP_OR: {
	xi = yi.Or(zi);
	break;
        }
        case OP_XOR: {
	xi = yi.Xor(zi);
	break;
        }
        case OP_SHL: {
	xi = yi.Shl(zi);
	break;
        }
        case OP_LSHR: {
          xi = yi.LShr(zi);
          break;
        }
        case OP_ASHR: {
	xi = yi.AShr(zi);
	break;
        }
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);
    }

    void apply(bitwise_operation_t op, VariableName x, VariableName y, Number k){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi(k);
      interval_t xi = interval_t::bottom();
      switch (op) {
        case OP_AND: {
	xi = yi.And(zi);
	break;
        }
        case OP_OR: {
	xi = yi.Or(zi);
	break;
        }
        case OP_XOR: {
	xi = yi.Xor(zi);
	break;
        }
        case OP_SHL: {
	xi = yi.Shl(zi);
	break;
        }
        case OP_LSHR: {
          xi = yi.LShr(zi);
          break;
        }
        case OP_ASHR: {
	xi = yi.AShr(zi);
	break;
        }
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);
    }

    // division_operators_api

    void apply(div_operation_t op, VariableName x, VariableName y, VariableName z){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi = this->_env[z];
      interval_t xi = interval_t::bottom();

      switch (op) {
        case OP_SDIV: {
	xi = yi / zi;
	break;
        }
        case OP_UDIV: {
	xi = yi.UDiv(zi);
	break;
        }
        case OP_SREM: {
	xi = yi.SRem(zi);
	break;
        }
        case OP_UREM: {
	xi = yi.URem(zi);
	break;
        }
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);

    }

    void apply(div_operation_t op, VariableName x, VariableName y, Number k){
      crab::CrabStats::count (getDomainName() + ".count.apply");
      crab::ScopedCrabStats __st__(getDomainName() + ".apply");

      interval_t yi = this->_env[y];
      interval_t zi(k);
      interval_t xi = interval_t::bottom();
      switch (op) {
        case OP_SDIV: {
	xi = yi / zi;
	break;
        }
        case OP_UDIV: {
	xi = yi.UDiv(zi);
	break;
        }
        case OP_SREM: {
	xi = yi.SRem(zi);
	break;
        }
        case OP_UREM: {
	xi = yi.URem(zi);
	break;
        }
        default:
          CRAB_ERROR("unreachable");
      }
      this->_env.set(x, xi);
    }

    void write(crab::crab_os& o) {
      this->_env.write(o);
    }

    linear_constraint_system_t to_linear_constraint_system ()
    {
      linear_constraint_system_t csts;

      if (this->is_bottom())
      {
        csts += linear_constraint_t (linear_expression_t (Number(1)) ==
                                     linear_expression_t (Number(0)));
        return csts;
      }

      for (iterator it = this->_env.begin(); it != this->_env.end(); ++it)
      {
        VariableName v = it->first;
        interval_t   i = it->second;
        boost::optional<Number> lb = i.lb().number();
        boost::optional<Number> ub = i.ub().number();
        if (lb) csts += linear_constraint_t( variable_t(v) >= *lb );
        if (ub) csts += linear_constraint_t( variable_t(v) <= *ub );
      }
      return csts;
    }

    static std::string getDomainName () {
      return "Intervals";
    }

  }; // class interval_domain

} // namespace ikos

#endif // IKOS_INTERVALS_HPP


#include <fcl/shape/geometric_shapes.h>
#include <fcl/shape/geometric_shapes_utility.h>
#include <fcl/narrowphase/narrowphase.h>
#include <iostream>
#include <fcl/collision.h>
#include <boost/foreach.hpp>

using namespace std;
using namespace fcl;

int main(int argc, char** argv)
{
  boost::shared_ptr<Box> box0(new Box(1,1,1));
  boost::shared_ptr<Box> box1(new Box(1,1,1));
//  GJKSolver_indep solver;
  GJKSolver_libccd solver;
  Vec3f contact_points;
  FCL_REAL penetration_depth;
  Vec3f normal;

  Transform3f tf0, tf1;
  tf0.setIdentity();
  tf0.setTranslation(Vec3f(.9,0,0));
  tf0.setQuatRotation(Quaternion3f(.6, .8, 0, 0));
  tf1.setIdentity();

  bool res = solver.shapeIntersect(*box0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal);

  cout << "contact points: " << contact_points << endl;
  cout << "pen depth: " << penetration_depth << endl;
  cout << "normal: " << normal << endl;
  cout << "result: " << res << endl;

  static const int num_max_contacts = std::numeric_limits<int>::max();
  static const bool enable_contact = true;
  fcl::CollisionResult result;
  fcl::CollisionRequest request(num_max_contacts,
                                enable_contact);

  CollisionObject co0(box0, tf0);
  CollisionObject co1(box1, tf1);

  fcl::collide(&co0, &co1, request, result);
  vector<Contact> contacts;
  result.getContacts(contacts);

  cout << contacts.size() << " contacts found" << endl;
  BOOST_FOREACH(Contact& contact, contacts) {
    cout << "position: " << contact.pos << endl;
  }
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGrid.h>
//#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPose3DPDFGrid>;

const auto bb_min =
	mrpt::math::TPose3D(1, 2, 3, -20.0_deg, -30.0_deg, -40.0_deg);
const auto bb_max = mrpt::math::TPose3D(3, 4, 5, 20.0_deg, 30.0_deg, 40.0_deg);

TEST(CPose3DPDFGrid, uniformDistribution)
{
	const auto gt_mean = mrpt::math::TPose3D(2, 3, 4, .0_deg, .0_deg, .0_deg);

	const double res_xyz = 0.25;
	const double res_YPR = mrpt::DEG2RAD(10.0);

	auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);

	grid.uniformDistribution();

	auto [COV, MEAN] = grid.getCovarianceAndMean();

	EXPECT_NEAR(MEAN.x(), gt_mean.x, 1e-4);
	EXPECT_NEAR(MEAN.y(), gt_mean.y, 1e-4);
	EXPECT_NEAR(MEAN.z(), gt_mean.z, 1e-4);

	EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 1e-4);
	EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 1e-4);
	EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 1e-4);

	for (int i = 0; i < 3; i++)
		EXPECT_GT(COV(i, i), mrpt::square(0.25));
	for (int i = 4; i < 6; i++)
		EXPECT_GT(COV(i, i), mrpt::square(mrpt::DEG2RAD(3.0)));
}

TEST(CPose3DPDFGrid, setManualPDF)
{
	const auto gt_mean =
		mrpt::math::TPose3D(2, 3, 4, 9.0_deg, 34.0_deg, 12.0_deg);

	const double res_xyz = 0.25;
	const double res_YPR = mrpt::DEG2RAD(10.0);

	auto grid = mrpt::poses::CPose3DPDFGrid(bb_min, bb_max, res_xyz, res_YPR);

	grid.fill(0);

	*grid.getByPos(gt_mean) = .1;

	grid.normalize();

	auto [COV, MEAN] = grid.getCovarianceAndMean();

	EXPECT_NEAR(MEAN.x(), gt_mean.x, 0.2);
	EXPECT_NEAR(MEAN.y(), gt_mean.y, 0.2);
	EXPECT_NEAR(MEAN.z(), gt_mean.z, 0.2);

	EXPECT_NEAR(MEAN.yaw(), gt_mean.yaw, 0.1);
	EXPECT_NEAR(MEAN.pitch(), gt_mean.pitch, 0.1);
	EXPECT_NEAR(MEAN.roll(), gt_mean.roll, 0.1);

	for (int i = 0; i < 6; i++)
		EXPECT_LT(COV(i, i), mrpt::square(0.05));
}


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/round.hpp>
#include <math/rev/scal/util.hpp>
#include <math/fwd/scal/fun/nan_util.hpp>
#include <math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdRound, FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::round;
  using stan::math::var;

  fvar<var> x(1.5, 1.3);
  fvar<var> a = round(x);

  EXPECT_FLOAT_EQ(round(1.5), a.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(0, g[0]);
}
TEST(AgradFwdRound, FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::round;
  using stan::math::var;

  fvar<var> x(1.5, 1.3);
  fvar<var> a = round(x);

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(0, g[0]);
}

TEST(AgradFwdRound, FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::round;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 1.5;
  x.val_.d_ = 2.0;

  fvar<fvar<var> > a = round(x);

  EXPECT_FLOAT_EQ(round(1.5), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0, g[0]);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.5;
  y.d_.val_ = 2.0;

  fvar<fvar<var> > b = round(y);
  EXPECT_FLOAT_EQ(round(1.5), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC q = createAVEC(y.val_.val_);
  VEC r;
  b.val_.val_.grad(q, r);
  EXPECT_FLOAT_EQ(0, r[0]);
}
TEST(AgradFwdRound, FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::round;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 1.5;
  x.val_.d_ = 2.0;

  fvar<fvar<var> > a = round(x);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0, g[0]);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.5;
  y.d_.val_ = 2.0;

  fvar<fvar<var> > b = round(y);

  AVEC q = createAVEC(y.val_.val_);
  VEC r;
  b.d_.val_.grad(q, r);
  EXPECT_FLOAT_EQ(0, r[0]);
}
TEST(AgradFwdRound, FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::round;
  using stan::math::var;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.5;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > b = round(y);

  AVEC q = createAVEC(y.val_.val_);
  VEC r;
  b.d_.d_.grad(q, r);
  EXPECT_FLOAT_EQ(0, r[0]);
}

struct round_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return round(arg1);
  }
};

TEST(AgradFwdRound, round_NaN_1) {
  round_fun round_;
  test_nan_mix(round_, false);
}


// The cell graph is a graph in which each vertex
// corresponds to a cell.
// An undirected edge is created between two vertices
// if the there is good similarity between the
// expression vectors of the corresponding cells.

#ifndef CZI_EXPRESSION_MATRIX2_CELL_GRAPH_HPP
#define CZI_EXPRESSION_MATRIX2_CELL_GRAPH_HPP

#include "CZI_ASSERT.hpp"
#include "Ids.hpp"
#include "orderPairs.hpp"

#include <boost/graph/adjacency_list.hpp>

#include "array.hpp"
#include "iosfwd.hpp"
#include "map.hpp"
#include "string.hpp"
#include "utility.hpp"
#include "vector.hpp"

namespace ChanZuckerberg {
    namespace ExpressionMatrix2 {

        class CellGraph;
        class CellGraphVertex;
        class CellGraphVertexInfo;
        class CellGraphEdge;
        class ClusterTable;

        // The base class for class CellGraph.
        typedef boost::adjacency_list<
            boost::listS,
            boost::listS,
            boost::undirectedS,
            CellGraphVertex,
            CellGraphEdge> CellGraphBaseClass;

        namespace MemoryMapped {
            template<class T> class Vector;
        }
    }
}

// A class used by label propagation algorithm to keep track
// of the total weight of each cluster for each vertex.
class ChanZuckerberg::ExpressionMatrix2::ClusterTable {
public:
    void addWeight(uint32_t clusterId, float weight);
    void addWeightQuick(uint32_t clusterId, float weight); // Does not check if already there. Does not update the best cluster.
    uint32_t bestCluster();
    void findBestCluster();
    void clear();
    bool isEmpty() const;

private:

    vector< pair<uint32_t, float> > data;

    uint32_t bestClusterId = std::numeric_limits<uint32_t>::max();
    float bestWeight = -1.;
};

inline void ChanZuckerberg::ExpressionMatrix2::ClusterTable::addWeightQuick(uint32_t clusterId, float weight)
{
    data.push_back(make_pair(clusterId, weight));
}
inline void ChanZuckerberg::ExpressionMatrix2::ClusterTable::addWeight(uint32_t clusterId, float weight)
{
    for(pair<uint32_t, float>& p: data) {
        if(p.first == clusterId) {
            p.second += weight;
            if(clusterId == bestClusterId) {
                if(weight < 0.) {
                    findBestCluster();
                } else {
                    bestWeight = p.second;
                }
            } else {
                if(p.second > bestWeight) {
                    bestClusterId = clusterId;
                    bestWeight = p.second;
                }
            }
            return;
        }
    }
    data.push_back(make_pair(clusterId, weight));
    if(weight > bestWeight) {
        bestClusterId = clusterId;
        bestWeight = weight;
    }
}
inline uint32_t ChanZuckerberg::ExpressionMatrix2::ClusterTable::bestCluster()
{
    return bestClusterId;
}
inline void ChanZuckerberg::ExpressionMatrix2::ClusterTable::findBestCluster()
{
    bestClusterId = std::numeric_limits<uint32_t>::max();
    bestWeight = -1.;
    for(const pair<uint32_t, float>& p: data) {
        if(p.second > bestWeight) {
            bestWeight = p.second;
            bestClusterId = p.first;
        }
    }
}
inline void ChanZuckerberg::ExpressionMatrix2::ClusterTable::clear()
{
    data.clear();
}
inline bool ChanZuckerberg::ExpressionMatrix2::ClusterTable::isEmpty() const
{
    return data.empty();
}

// A vertex of the cell graph.
// The base class CellGraphVertexInfo is used to communicate with Python.
class ChanZuckerberg::ExpressionMatrix2::CellGraphVertexInfo {
public:
    CellId cellId = invalidCellId;
    array<double, 2> position;
    ClusterTable clusterTable;
    double x() const
    {
        return position[0];
    }
    double y() const
    {
        return position[1];
    }
    CellGraphVertexInfo()
    {
    }
    CellGraphVertexInfo(const CellGraphVertexInfo& that) :
        cellId(that.cellId), position(that.position)
    {
    }
    CellGraphVertexInfo(CellId cellId) :
        cellId(cellId)
    {
    }
    bool operator==(const CellGraphVertexInfo& that)
    {
        return cellId==that.cellId && position==that.position;
    }
};
class ChanZuckerberg::ExpressionMatrix2::CellGraphVertex : public CellGraphVertexInfo {
public:

    // Use the base class constructors.
    using CellGraphVertexInfo::CellGraphVertexInfo;

    // Additional fields not needed in Python.
    uint32_t group = 0;
    uint32_t clusterId = 0;
    string color;
    double value = 0.;
};

// An edge of the cell graph.
class ChanZuckerberg::ExpressionMatrix2::CellGraphEdge {
public:
    float similarity = -1.;

    CellGraphEdge()
    {
    }

    CellGraphEdge(float similarity) :
        similarity(similarity)
    {
    }

    string color;
};

class ChanZuckerberg::ExpressionMatrix2::CellGraph : public CellGraphBaseClass {
public:

    // Use the constructors of the base class.
    using CellGraphBaseClass::CellGraphBaseClass;

    typedef CellGraph Graph;
    Graph& graph()
    {
        return *this;
    }
    const Graph& graph() const
    {
        return *this;
    }

    CellGraph(
        const MemoryMapped::Vector<CellId>& cellSet, // The cell set to be used.
        const string& similarPairsName,           // The name of the SimilarPairs object to be used to create the graph.
        double similarityThreshold,                  // The minimum similarity to create an edge.
        size_t maxConnectivity                       // The maximum number of neighbors (k of the k-NN graph).
        );

    // Only keep an edge if it is one of the best k edges for either
    // of the two vertices. This turns the graph into a k-nearest-neighbor graph.
    void keepBestEdgesOnly(std::size_t k);

    // Write in Graphviz format.
    void write(ostream&) const;
    void write(const string& fileName) const;

    // Simple graph statistics.
    ostream& writeStatistics(ostream&) const;

    // Remove isolated vertices and returns\ the number of vertices that were removed
    size_t removeIsolatedVertices();

    // Use Graphviz to compute the graph layout and store it in the vertex positions.
    void computeLayout();
    bool layoutWasComputed = false;

    // Clustering using the label propagation algorithm.
    // The cluster each vertex is assigned to is stored in the clusterId data member of the vertex.
    void labelPropagationClustering(
        ostream&,
        size_t seed,                            // Seed for random number generator.
        size_t stableIterationCountThreshold,   // Stop after this many iterations without changes.
        size_t maxIterationCount                // Stop after this many iterations no matter what.
        );

    // Vertex table, keyed by cell id.
    map<CellId, vertex_descriptor> vertexTable;

    // Compute minimum and maximum coordinates of all the vertices.
    void computeCoordinateRange(
        double& xMin,
        double& xMax,
        double& yMin,
        double& yMax) const;

    // Assign integer colors to groups.
    // The same color can be used for multiple groups, but if two
    // groups are joined by one or more edges they must have distinct colors.
    // On return, colorTable[group] contains the integer color assigned to each group.
    // This processes the groups in increasing order beginning at group 0,
    // so it is best if the group numbers are all contiguous, starting at zero,
    // and in decreasing size of group.
    void assignColorsToGroups(vector<uint32_t>& colorTable);

    // Write the graph in svg format.
    // This does not use Graphviz. It uses the graph layout stored in the vertices,
    // and previously computed using Graphviz.
    // The last argument specifies the color assigned to each vertex group.
    // If empty, vertex groups are not used, and each vertex is drawn
    // with its own color.
    void writeSvg(
        ostream& s,
        bool hideEdges,
        double svgSizePixels,
        double xViewBoxCenter,
        double yViewBoxCenter,
        double viewBoxHalfSize,
        double vertexRadius,
        double edgeThickness,
        const map<int, string>& groupColors,
        const string& geneSetName   // Used for the cell URL
        ) const;

    class Writer {
    public:
        Writer(const Graph&);
        void operator()(ostream&) const;
        void operator()(ostream&, vertex_descriptor) const;
        void operator()(ostream&, edge_descriptor) const;
        const Graph& graph;
        double minEdgeSimilarity;
    };
};

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_HALF_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_HALF_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Half generic tag

     Represents the Half constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER(Half,double,0,0x3F000000,0x3FE0000000000000ULL)
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Half, Site> dispatching_Half(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Half, Site>();
   }
   template<class... Args>
   struct impl_Half;
  }
  /*!
    Generates value \f$\frac12\f$

    @par Semantic:

    @code
    T r = Half<T>();
    @endcode

    is similar for floating types to:

    @code
    T r = T(1)/T(2);
    @endcode

    and returns 0 for integral types

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Half, Half)
} }

#include <boost/simd/constant/common.hpp>

#endif


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

#ifndef NETKET_HILBERT_INDEX_HPP
#define NETKET_HILBERT_INDEX_HPP

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <vector>

#include "Hamiltonian/ising.hpp"
#include "hilbert.hpp"

namespace netket {

class HilbertIndex {
  const std::vector<double> localstates_;

  const int localsize_;

  const int size_;

  std::map<double, int> statenumber_;

  std::vector<std::size_t> basis_;

  int nstates_;

 public:
  explicit HilbertIndex(const Hilbert &hilbert)
      : localstates_(hilbert.LocalStates()),
        localsize_(hilbert.LocalSize()),
        size_(hilbert.Size()) {
    Init();
  }

  void Init() {
    if (size_ * std::log(localsize_) > std::log(MaxStates)) {
      throw InvalidInputError("Hilbert space is too large to be indexed");
    }

    nstates_ = std::pow(localsize_, size_);

    std::size_t ba = 1;
    for (int s = 0; s < size_; s++) {
      basis_.push_back(ba);
      ba *= localsize_;
    }

    for (std::size_t k = 0; k < localstates_.size(); k++) {
      statenumber_[localstates_[k]] = k;
    }
  }

  // converts a vector of quantum numbers into the unique integer identifier
  std::size_t StateToNumber(const Eigen::VectorXd &v) const {
    std::size_t number = 0;

    for (int i = 0; i < size_; i++) {
      assert(statenumber_.count(v(size_ - i - 1)) > 0);
      number += statenumber_.at(v(size_ - i - 1)) * basis_[i];
    }

    return number;
  }

  // converts a vector of quantum numbers into the unique integer identifier
  // this version assumes that a number representaiton is already known for the
  // given vector v, and this function is used to update it
  std::size_t DeltaStateToNumber(const Eigen::VectorXd &v,
                                 nonstd::span<const int> connector,
                                 nonstd::span<const double> newconf) const {
    std::size_t number = 0;

    for (int k = 0; k < connector.size(); k++) {
      const int ich = connector[k];
      number -= statenumber_.at(v(ich)) * basis_[size_ - ich - 1];
      number += statenumber_.at(newconf[k]) * basis_[size_ - ich - 1];
    }

    return number;
  }

  // converts an integer into a vector of quantum numbers
  Eigen::VectorXd NumberToState(std::size_t i) const {
    Eigen::VectorXd result = Eigen::VectorXd::Constant(size_, localstates_[0]);

    std::size_t ip = i;

    int k = size_ - 1;

    while (ip > 0) {
      result(k) = localstates_[ip % localsize_];
      ip /= localsize_;
      k--;
    }
    return result;
  }

  std::size_t NStates() const { return nstates_; }

  constexpr static int MaxStates = std::numeric_limits<int>::max() - 1;
};

}  // namespace netket
#endif


// FLAGS:= -I/opt/local/boost/include/ -L/opt/local/boost/lib/ -O3
#include <stdio.h>
#include <sys/time.h>

/*
---- @TCK ----
Add       : 0.309122
Clear     : 0.400807
Aggr      : 0.090225
Update    : 0.107135
Update2   : 0.496113
*/

#include <boost/fusion/tuple.hpp>
#include <boost/fusion/include/fold.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/multi_index_container.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/sequenced_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/mem_fun.hpp>

using namespace ::std;
using namespace ::boost;
using namespace ::boost::serialization;
using namespace ::boost::fusion;
using namespace ::boost::lambda;
using namespace ::boost::multi_index;

// http://stackoverflow.com/questions/1250599/how-to-unordered-settupleint-int

// Tuples hashing function
struct tuple_fusion_hash { typedef std::size_t result_type; template<typename T> std::size_t operator()(std::size_t seed, const T& arg) { boost::hash_combine(seed, arg); return seed; } };
struct tuple_hash { template<typename T> size_t operator()(const T& t) const { return boost::fusion::fold(t, 0, tuple_fusion_hash()); } };

// Secondary indices
//struct Idx0 {}; struct Idx1 {}; struct Idx2 {}; struct Idx3 {}; struct Idx4 {};

// My map definition
typedef boost::fusion::tuple<long,long,string> Key;
typedef boost::fusion::tuple<double,double> Val;

struct Rec {
  Key key; Val val;
  Rec(long c0,long c1,string c2, double c3, double c4) { key=Key(c0,c1,c2); val=Val(c3,c4); }
  Rec(const Key& k, const Val& v) { key=k; val=v; }
  Rec(std::pair<const Key,const Val>& p) { key=p.first; val=p.second; }
  // operator const std::pair<const Key, long>() const { return std::make_pair(key,val); }
  //long idx0() const { return get<0>(key); } // secondary indices
};

typedef multi_index_container<Rec, indexed_by<
  hashed_unique<member<Rec,Key,&Rec::key>,tuple_hash> //,
  //hashed_non_unique<tag<Idx0>, const_mem_fun<Rec,long,&Rec::idx0> > // secondary indices
> > MapBase;

class Map : public MapBase {
public:
  //typedef index<Idx0>::type::iterator it0;
  void clear() { MapBase::clear_(); }
};

Map map1;
Map map2;
static long N = 1000000;

void load() { for (long i=0;i<N;++i) map1.insert(Rec(i,i+1,"Hello world", i*3.0,i*4.0)); }
void clear() { map1.clear(); map2.clear(); }

int dcmp(const void *x, const void *y) { double xx = *(double*)x, yy = *(double*)y; return (xx<yy) ? -1 : ((xx > yy) ? 1 : 0); }

struct Timer {
  int n;
  double res[20];
  struct timeval t0,t1;
  Timer() { n=0; }
  void start() { gettimeofday (&t0,NULL); }
  void stop() {
    gettimeofday (&t1,NULL);
    res[n]=(t1.tv_sec-t0.tv_sec)+((double)(t1.tv_usec-t0.tv_usec))/1000000L;
    if (n<19) ++n;
  }
  void reset() { n=0; }
  void pr(const char* name) { qsort(res,n,sizeof(double),dcmp); printf("%-10s: %0.6f\n",name,res[n/2]); }
};

struct Fun_xx {
  void operator()(Rec &r) {
    r.val = Val(get<0>(r.val)*2.0,get<1>(r.val)*0.5);
  }
};

int main() {
  // 1. Add/clear
  Timer t_add;
  Timer t_clear;
  for (int z=0;z<10;++z) {
    t_add.start(); load(); t_add.stop();
    t_clear.start(); clear(); t_clear.stop();
  }
  t_add.pr("Add");
  t_clear.pr("Clear");
  load();

  // 2. Loop/aggregate
  Timer t_aggr;
  double acc=0;
  for (int z=0;z<10;++z) {
    t_aggr.start();
    Map::iterator it,end;
    it=map1.begin(); end=map1.end();
    if (it!=end) do {
      //map1.erase(it);
      acc += get<0>(it->val)*2.0+get<1>(it->val)*0.5;
    } while (++it!=end);
    t_aggr.stop();
  }
  t_aggr.pr("Aggr");

  // 3. Loop update
  Timer t_updt;
  for (int z=0;z<10;++z) {
    t_updt.start();
    Map::iterator it,end;
    it=map1.begin(); end=map1.end();
    if (it!=end) do {
      Fun_xx x;
      map1.modify(it,x);
    } while (++it!=end);
    t_updt.stop();
  }
  t_updt.pr("Update");

  // 4. Loop add/delete
  Timer t_up2;
  for (int z=0;z<10;++z) {
    t_up2.start();
    Map::iterator it,end;
    if (z%2==0) {
      it=map1.begin(); end=map1.end();
    } else {
      it=map2.begin(); end=map2.end();
    }
    if (it!=end) do {
      Rec r( get<0>(it->key)-3, get<1>(it->key)+3, "Hello world2", get<0>(it->val)*2.0, get<1>(it->key)*0.5  );
      if (z%2==0) {
        map2.insert(r);
        map1.erase(it);
      } else {
        map1.insert(r);
        map2.erase(it);
      }
    } while (++it!=end);
    t_up2.stop();
  }
  t_up2.pr("Update2");

  return 0;
}


/*
 * GradientDescent.h
 *
 *  Created on: Dec 19, 2016
 *      Author: ken
 */

#ifndef LAYER_OPTIMIZER_SGD_HPP_
#define LAYER_OPTIMIZER_SGD_HPP_

#include "Optimizer.hpp"
#include <armadillo>

class SGD: public Optimizer {
private:
	double mEta;
public:
	SGD(double eta);
	virtual ~SGD();
	arma::field<arma::Cube<double>> delta(const arma::field<arma::Cube<double>>& gradients, unsigned int batchSize);
};

#endif /* LAYER_OPTIMIZER_SGD_HPP_ */


#pragma once

#include <boost/container_hash/extensions.hpp>
#include <boost/container_hash/hash_fwd.hpp>
#include <boost/functional/hash.hpp>
#include <vector>

struct Point
{
	int x, y;

	Point(int x, int y)
		:	x{x}, y{y}
	{}

	friend std::size_t hash_value(const Point& p)
	{
		std::size_t seed{0x1e4493f};
		boost::hash_combine(seed, p.x);
		boost::hash_combine(seed, p.y);
		return seed;
	}
};

struct Line
{
	Point begin, end;

	Line(Point a, Point b)
		:	begin{std::move(a)},
			end{std::move(b)}
	{}

	friend std::size_t hash_value(const Line& p)
	{
		std::size_t seed{0x52e4f07a};
		boost::hash_combine(seed, p.begin);
		boost::hash_combine(seed, p.end);
		return seed;
	}
};

struct VectorObject
{
	virtual std::vector<Line>::iterator begin() = 0;
	virtual std::vector<Line>::iterator end() = 0;
};

class VectorRectangle : public VectorObject
{
	std::vector<Line> m_lines;

public:
	VectorRectangle(int x, int y, int width, int height)
		:	m_lines{
				{{x, y}, {x + width, y}},
				{{x + width, y}, {x + width, y + height}},
				{{x, y}, {x, y + height}},
				{{x, y + height}, {x + width, y + height}},
			}
	{}

	std::vector<Line>::iterator begin() override
	{
		return m_lines.begin();
	}

	std::vector<Line>::iterator end() override
	{
		return m_lines.end();
	}
};

inline std::vector<Point> line_to_points(const Line& line)
{
	std::vector<Point> points;

	auto left{std::min(line.begin.x, line.end.x)};
	auto right{std::max(line.begin.x, line.begin.y)};
	auto bottom{std::min(line.begin.y, line.end.y)};
	auto top{std::max(line.begin.y, line.end.y)};
	auto dx{right - left}, dy{line.end.y - line.begin.y};

	if(dx == 0) // vertical
	{
		for(int y = top; y < bottom; --y)
		{
			points.emplace_back(left, y);
		}
	}
	else if (dy == 0) // horizontal
	{
		for(int x = left; x < right; ++x)
		{
			points.emplace_back(x, top);
		}
	}
	else // TODO: Non-vertical/horizontal line support
	{}

	return points;
}


#include "rbfcore.h"
#include "utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <iomanip>
#include <ctime>
#include <chrono>
#include<algorithm>

double sigma = 2.0;
double inv_sigma_squarex2 = 1 / (2 * pow(sigma, 2));

double Gaussian_Kernel(const double x_square) {
    return exp(-x_square * inv_sigma_squarex2);
}

double Gaussian_Kernel_2p(const double *p1, const double *p2) {
    return Gaussian_Kernel(MyUtility::vecSquareDist(p1, p2));
}

double Gaussian_PKernel_Dirichlet_2p(const double *p1, const double *p2) {
    double d2 = MyUtility::vecSquareDist(p1, p2);
    return (6 * sigma * sigma - d2) * sqrt(Gaussian_Kernel(d2));
}

double Gaussian_PKernel_Bending_2p(const double *p1, const double *p2) {

    double d2 = MyUtility::vecSquareDist(p1, p2);
    double d4 = d2 * d2;
    double sigma2 = sigma * sigma;
    double sigma4 = sigma2 * sigma2;
    return (60 * sigma4 - 20 * sigma2 * d2 + d4) * sqrt(Gaussian_Kernel(d2));

}

double XCube_Kernel(const double x) {

    return pow(x, 3);
}

double XCube_Kernel_2p(const double *p1, const double *p2) {

    return XCube_Kernel(MyUtility::_VerticesDistance(p1, p2));

}

void XCube_Gradient_Kernel_2p(const double *p1, const double *p2, double *G) {
    double len_dist = MyUtility::_VerticesDistance(p1, p2);
    for (int i = 0; i < 3; ++i)
        G[i] = 3 * len_dist * (p1[i] - p2[i]);
    return;

}

double XCube_GradientDot_Kernel_2p(const double *p1, const double *p2, const double *p3) {

    double G[3];
    XCube_Gradient_Kernel_2p(p1, p2, G);
    return MyUtility::dot(p3, G);

}

void XCube_Hessian_Kernel_2p(const double *p1, const double *p2, double *H) {

    double diff[3];
    for (int i = 0; i < 3; ++i)diff[i] = p1[i] - p2[i];
    double len_dist = sqrt(MyUtility::len(diff));

    if (len_dist < 1e-8) {
        for (int i = 0; i < 9; ++i)H[i] = 0;
    } else {
        for (int i = 0; i < 3; ++i) {
            H[i * 3 + i] = 3 * pow(diff[i], 2) / len_dist + 3 * len_dist;
            for (int j = i + 1; j < 3; ++j) {
                H[j * 3 + i] = H[i * 3 + j] = 3 * diff[i] * diff[j] / len_dist;
            }
        }
    }

    return;

}

void XCube_HessianDot_Kernel_2p(const double *p1, const double *p2, const double *p3, vector<double> &dotout) {

    double H[9];
    XCube_Gradient_Kernel_2p(p1, p2, H);
    dotout.resize(3);
    for (int i = 0; i < 3; ++i) {
        dotout[i] = 0;
        for (int j = 0; j < 3; ++j) {
            dotout[i] += H[i * 3 + j] * p3[j];
        }
    }

}

RBF_Core::RBF_Core() {

    Kernal_Function = Gaussian_Kernel;
    Kernal_Function_2p = Gaussian_Kernel_2p;
    P_Function_2p = Gaussian_PKernel_Dirichlet_2p;

    isHermite = false;

    mp_RBF_INITMETHOD.insert(make_pair(GT_NORMAL, "GT_NORMAL"));
    mp_RBF_INITMETHOD.insert(make_pair(GlobalEigen, "GlobalEigen"));
    mp_RBF_INITMETHOD.insert(make_pair(GlobalEigenWithMST, "GlobalEigenWithMST"));
    mp_RBF_INITMETHOD.insert(make_pair(GlobalEigenWithGT, "GlobalEigenWithGT"));
    mp_RBF_INITMETHOD.insert(make_pair(LocalEigen, "LocalEigen"));
    mp_RBF_INITMETHOD.insert(make_pair(IterativeEigen, "IterativeEigen"));
    mp_RBF_INITMETHOD.insert(make_pair(ClusterEigen, "ClusterEigen"));
    mp_RBF_INITMETHOD.insert(make_pair(Lamnbda_Search, "LamnbdaSearch"));

    mp_RBF_METHOD.insert(make_pair(Variational, "Variational"));
    mp_RBF_METHOD.insert(make_pair(Variational_P, "Variation_P"));
    mp_RBF_METHOD.insert(make_pair(LS, "LS"));
    mp_RBF_METHOD.insert(make_pair(LSinterp, "LSinterp"));
    mp_RBF_METHOD.insert(make_pair(Interp, "Interp"));
    mp_RBF_METHOD.insert(make_pair(RayleighQuotients, "Rayleigh"));
    mp_RBF_METHOD.insert(make_pair(RayleighQuotients_P, "Rayleigh_P"));
    mp_RBF_METHOD.insert(make_pair(RayleighQuotients_I, "Rayleigh_I"));
    mp_RBF_METHOD.insert(make_pair(Hermite, "Hermite"));
    mp_RBF_METHOD.insert(make_pair(Hermite_UnitNorm, "UnitNorm"));
    mp_RBF_METHOD.insert(make_pair(Hermite_UnitNormal, "UnitNormal"));
    mp_RBF_METHOD.insert(make_pair(Hermite_Tangent_UnitNorm, "T_UnitNorm"));
    mp_RBF_METHOD.insert(make_pair(Hermite_Tangent_UnitNormal, "T_UnitNormal"));

    mp_RBF_Kernal.insert(make_pair(XCube, "TriH"));
    mp_RBF_Kernal.insert(make_pair(ThinSpline, "ThinSpline"));
    mp_RBF_Kernal.insert(make_pair(XLinear, "XLinear"));
    mp_RBF_Kernal.insert(make_pair(Gaussian, "Gaussian"));

}

RBF_Core::RBF_Core(RBF_Kernal kernal) {
    isHermite = false;
    Init(kernal);
}

void RBF_Core::Init(RBF_Kernal kernal) {

    this->kernal = kernal;
    switch (kernal) {
        case Gaussian:
            Kernal_Function = Gaussian_Kernel;
            Kernal_Function_2p = Gaussian_Kernel_2p;
            P_Function_2p = Gaussian_PKernel_Dirichlet_2p;
            break;

        case XCube:
            Kernal_Function = XCube_Kernel;
            Kernal_Function_2p = XCube_Kernel_2p;
            Kernal_Gradient_Function_2p = XCube_Gradient_Kernel_2p;
            Kernal_Hessian_Function_2p = XCube_Hessian_Kernel_2p;
            break;

        default:
            break;

    }

}

void RBF_Core::SetSigma(double x) {
    sigma = x;
    inv_sigma_squarex2 = 1 / (2 * pow(sigma, 2));
}

double RBF_Core::Dist_Function(const double x, const double y, const double z) {
    return RBF_Core::Dist_Function(R3Pt(x, y, z));
}

inline double RBF_Core::Dist_Function(const double *p) {
    n_evacalls++;
    double *p_pts = pts.data();
    static arma::vec kern(npt), kb;
    if (isHermite) {
        kern.set_size(npt * 4);
        double G[3];
        for (int i = 0; i < npt; ++i) {
            kern(i) = Kernal_Function_2p(p_pts + i * 3, p);
            Kernal_Gradient_Function_2p(p, p_pts + i * 3, G);
            //for(int j=0;j<3;++j)kern(npt+i*3+j) = -G[j];
            for (int j = 0; j < 3; ++j)
                kern(npt + i * 3 + j) = G[j];
        }
    } else {
//        kern.set_size(npt);
//        for (int i = 0; i < npt; ++i)
//            kern(i) = Kernal_Function_2p(p_pts + i * 3, p);
    }

    double loc_part = dot(kern, a);

    if (polyDeg == 1) {
        //here for hermite
        kb.set_size(4);
        for (int i = 0; i < 3; ++i)
            kb(i + 1) = p[i];
        kb(0) = 1;
    } else if (polyDeg == 2) {
//        vector<double> buf(4, 1);
//        int ind = 0;
//        kb.set_size(10);
//        for (int j = 0; j < 3; ++j)buf[j + 1] = p[j];
//        for (int j = 0; j < 4; ++j)
//            for (int k = j; k < 4; ++k)
//                kb(ind++) = buf[j] * buf[k];
        // kb = 1,x,y,z,xx,xy,xz,yy,yz,zz

    }
    double poly_part = dot(kb, b);

    if (0) {
        cout << "dist: " << p[0] << ' ' << p[1] << ' ' << p[2] << ' ' << p_pts[3] << ' ' << p_pts[4] << ' ' << p_pts[5]
             << ' ' <<
             Kernal_Function_2p(p, p_pts + 3) << endl;
        for (int i = 0; i < npt; ++i)cout << kern(i) << ' ';
        for (int i = 0; i < bsize; ++i)cout << kb(i) << ' ';
        cout << endl;
    }

    double re = loc_part + poly_part;
    return re;

}

static RBF_Core *s_hrbf;

double RBF_Core::Dist_Function(const R3Pt &in_pt) {
    return s_hrbf->Dist_Function(&(in_pt[0]));
}

//FT RBF_Core::Dist_Function(const Point_3 in_pt){

//    return s_hrbf->Dist_Function(&(in_pt.x()));
//}

void RBF_Core::SetThis() {

    s_hrbf = this;
}

void RBF_Core::Write_Surface(string fname) {

    //writeObjFile(fname,finalMesh_v,finalMesh_fv);

    writePLYFile_VF(fname, finalMesh_v, finalMesh_fv);
}

/**********************************************************/

void RBF_Core::Record(RBF_METHOD method, RBF_Kernal kernal, Solution_Struct &rsol, double time) {

    npoints.push_back(npt);

    record_initmethod.push_back(mp_RBF_INITMETHOD[curInitMethod]);
    record_method.push_back(mp_RBF_METHOD[method]);
    record_kernal.push_back(mp_RBF_Kernal[kernal]);
    record_initenergy.push_back(rsol.init_energy);
    record_energy.push_back(rsol.energy);
    record_time.push_back(time);

    setup_timev.push_back(setup_time);
    init_timev.push_back(init_time);
    solve_timev.push_back(solve_time);
    callfunc_timev.push_back(callfunc_time);
    invM_timev.push_back(invM_time);
    setK_timev.push_back(setK_time);

}

void RBF_Core::Record() {

    //cout<<"record"<<endl;
    npoints.push_back(npt);

    record_initmethod.push_back(mp_RBF_INITMETHOD[curInitMethod]);
    record_method.push_back(mp_RBF_METHOD[curMethod]);
    //record_kernal.push_back(mp_RBF_Kernal[kernal]);
    record_initenergy.push_back(sol.init_energy);
    record_energy.push_back(sol.energy);
    //cout<<"record"<<endl;

//    setup_timev.push_back(setup_time);
//    init_timev.push_back(init_time);
//    solve_timev.push_back(solve_time);
//    callfunc_timev.push_back(callfunc_time);
//    invM_timev.push_back(invM_time);
//    setK_timev.push_back(setK_time);
    // cout<<"record end"<<endl;
}

void RBF_Core::AddPartition(string pname) {

    record_partition.push_back(record_method.size());
    record_partition_name.push_back(pname);
}

void RBF_Core::Print_Record() {

    cout << "Method\t\t Kernal\t\t Energy\t\t Time" << endl;
    cout << std::setprecision(8) << endl;
    if (record_partition.size() == 0) {
        for (int i = 0; i < record_method.size(); ++i) {
            cout << record_method[i] << "\t\t" << record_kernal[i] << "\t\t" << record_energy[i] << "\t\t"
                 << record_time[i] << endl;
        }
        for (int i = 0; i < setup_timev.size(); ++i) {
            cout << setup_timev[i] << "\t\t" << init_timev[i] << "\t\t" << solve_timev[i] << "\t\t" << callfunc_timev[i]
                 << "\t\t" << invM_timev[i] << "\t\t" << setK_timev[i] << endl;
        }
    } else {
        for (int j = 0; j < record_partition.size(); ++j) {
            cout << record_partition_name[j] << endl;
            for (int i = j == 0 ? 0 : record_partition[j - 1]; i < record_partition[j]; ++i) {
                cout << record_method[i] << "\t\t" << record_kernal[i] << "\t\t" << record_energy[i] << "\t\t"
                     << record_time[i] << endl;
            }
            for (int i = j == 0 ? 0 : record_partition[j - 1]; i < record_partition[j]; ++i) {
                cout << setup_timev[i] << "\t\t" << init_timev[i] << "\t\t" << solve_timev[i] << "\t\t"
                     << callfunc_timev[i] << endl;
            }
        }
    }

}

void RBF_Core::Print_TimerRecord(string fname) {

    ofstream fout(fname);
    fout << setprecision(5);
    if (!fout.fail()) {
        for (int i = 0; i < setup_timev.size(); ++i) {
            fout << npoints[i] << '\t' << setup_timev[i] << "\t" << init_timev[i] << "\t" << solve_timev[i] << "\t"
                 << callfunc_timev[i] << "\t" << invM_timev[i] << "\t" << setK_timev[i] << endl;
        }
    }
    fout.close();

}

void RBF_Core::Print_TimerRecord_Single(string fname) {

    ofstream fout(fname);
    fout << setprecision(5);
    if (!fout.fail()) {
        fout << "number of points: " << npt << endl
             << "setup_time (Compute H): " << setup_time << " s" << endl
             << "init_time (Optimize g/Eigen): " << init_time << " s" << endl
             << "solve_time (Optimize g/LBFGS): " << solve_time << " s" << endl
             << "surfacing_time: " << surf_time << " s" << endl;
    }
    fout.close();
}

void RBF_Core::Clear_TimerRecord() {
    npoints.clear();
    setup_timev.clear();
    init_timev.clear();
    solve_timev.clear();
    callfunc_timev.clear();
    invM_timev.clear();
    setK_timev.clear();

}


#include <trajopt_sco/solver_interface.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_utils/stl_to_string.hpp>
#include <gtest/gtest.h>
#include <cstdio>
#include <boost/foreach.hpp>
#include <iostream>

using namespace std;

using namespace sco;

TEST(solver_interface, setup_problem) {
  ModelPtr solver = createModel();
  vector<Var> vars;
  for (int i=0; i < 3; ++i) {
    char namebuf[5];
    sprintf(namebuf, "v%i", i);
    vars.push_back(solver->addVar(namebuf));
  }
  solver->update();

  AffExpr aff;
  cout << aff << endl;
  for (int i=0; i < 3; ++i) {
    exprInc(aff, vars[i]);
    solver->setVarBounds(vars[i], 0, 10);
  }
  aff.constant -= 3;
  cout << aff << endl;
  QuadExpr affsquared = exprSquare(aff);
  solver->setObjective(affsquared);
  solver->update();
  LOG_INFO("objective: %s", CSTR(affsquared));
  LOG_INFO("please manually check that /tmp/solver-interface-test.lp matches this");
  solver->writeToFile("/tmp/solver-interface-test.lp");

  solver->optimize();

  vector<double> soln(3);
  for (int i=0; i < 3; ++i) {
    soln[i] = solver->getVarValue(vars[i]);
  }
  EXPECT_NEAR(aff.value(soln), 0, 1e-6);

  solver->removeVar(vars[2]);
  solver->update();
  EXPECT_EQ(solver->getVars().size(), 2);

}


/*
 * split_denoise.cpp

 *
 *  Created on: Dec 10, 2015
 *      Author: dch
 */

#include "hoNDArray_fileio.h"
#include "hoCuNDArray_math.h"
#include "vector_td_utilities.h"
#include "hoNDArray_utils.h"
#include "GPUTimer.h"
#include "cuATrousOperator.h"
#include "cuEdgeATrousOperator.h"
#include "cuSbCgSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <boost/program_options.hpp>
#include "subsetAccumulateOperator.h"

#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/make_shared.hpp>
#include <operators/cuGaussianFilterOperator.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "accumulateOperator.h"
#include "subselectionOperator.h"
#include "subsetConverter.h"
#include "nonlocalMeans.h"
using namespace std;
using namespace Gadgetron;
using namespace std;

namespace po = boost::program_options;
int main(int argc, char** argv){

	po::options_description desc("Allowed options");
	unsigned int iterations;
	int device;
	float noise;
	string outputFile;
	desc.add_options()
    				("help", "produce help message")
    				("input,a", po::value<string>(), "Input filename")
    				("output,f", po::value<string>(&outputFile)->default_value("denoised.real"), "Output filename")
    				("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    				("noise",po::value<float>(&noise)->default_value(1),"noise level")

    				;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	string filename = vm["input"].as<string>();

	auto hoInput = read_nd_array<float>(filename.c_str());

	auto input = cuNDArray<float>(*hoInput);

	auto output = input;
	clear(&output);

	size_t elements = input.get_size(0)*input.get_size(1);
	int slices = input.get_number_of_elements()/elements;
	std::vector<size_t> dims {input.get_size(0),input.get_size(1)};
	for (int i = 0; i < slices; i++) {
//		std::cout << "Processing slice " << i << std::endl;
		auto input_view = cuNDArray<float>(dims,input.get_data_ptr()+i*elements);
		auto output_view = cuNDArray<float>(dims,output.get_data_ptr()+i*elements);
		nonlocal_means_block2DPoisson(&input_view,&output_view,noise);
//		nonlocal_means2DPoisson(&input_view, &output_view, noise);

	}

	write_nd_array(&output,outputFile.c_str());

}


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/laplacian.h>
#include <cinolib/symbols.h>
#include <Eigen/Sparse>

namespace cinolib
{

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<Eigen::Triplet<double>> laplacian_matrix_entries(const AbstractMesh<M,V,E,P> & m, const int mode)
{
    std::vector<Entry> entries;
    for(uint vid=0; vid<m.num_verts(); ++vid)
    {
        std::vector<std::pair<uint,double>> wgts;
        m.vert_weights(vid, mode, wgts);

        double sum = 0.0;
        for(auto item : wgts)
        {
            entries.push_back(Entry(vid, item.first, item.second));
            sum -= item.second;
        }
        if (sum == 0.0)
        {
            std::cerr << "WARNING: null row in the matrix! (disconnected vertex? I put 1 in the diagonal)" << std::endl;
            sum = 1.0;
        }
        entries.push_back(Entry(vid, vid, sum));
    }

    return entries;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<Eigen::Triplet<double>> laplacian_3d_matrix_entries(const AbstractMesh<M,V,E,P> & m, const int mode)
{
    std::vector<Entry> entries;

    uint nv     = m.num_verts();
    uint base_x = nv * 0;
    uint base_y = nv * 1;
    uint base_z = nv * 2;

    for(uint vid=0; vid<m.num_verts(); ++vid)
    {
        std::vector<std::pair<uint,double>> wgts;
        m.vert_weights(vid, mode, wgts);
        double sum = 0.0;
        for(auto item : wgts)
        {
            entries.push_back(Entry(base_x + vid, base_x + item.first, item.second));
            entries.push_back(Entry(base_y + vid, base_y + item.first, item.second));
            entries.push_back(Entry(base_z + vid, base_z + item.first, item.second));
            sum -= item.second;
        }
        if (sum == 0.0)
        {
            std::cerr << "WARNING: null row in the matrix! (disconnected vertex? I put 1 in the diagonal)" << std::
                         endl;
            sum = 1.0;
        }
        entries.push_back(Entry(base_x + vid, base_x + vid, sum));
        entries.push_back(Entry(base_y + vid, base_y + vid, sum));
        entries.push_back(Entry(base_z + vid, base_z + vid, sum));
    }

    return entries;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
Eigen::SparseMatrix<double> laplacian(const AbstractMesh<M,V,E,P> & m, const int mode)
{
    std::vector<Entry> entries = laplacian_matrix_entries(m, mode);

    Eigen::SparseMatrix<double> L(m.num_verts(), m.num_verts());
    L.setFromTriplets(entries.begin(), entries.end());

    return L;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
Eigen::SparseMatrix<double> laplacian_3d(const AbstractMesh<M,V,E,P> & m, const int mode)
{
    std::vector<Entry> entries = laplacian_3d_matrix_entries(m, mode);

    Eigen::SparseMatrix<double> L(m.num_verts() * 3, m.num_verts() * 3);
    L.setFromTriplets(entries.begin(), entries.end());

    return L;
}

}