///////////////////////////////////////////////////////////////////////////////
// Authors: Chester Holtz, Devon Merrill, James (Ting-Chou) Lin, Connie (Yen-Yi) Wu
//          (respective Ph.D. advisors: Chung-Kuan Cheng, Andrew B. Kahng, Steven Swanson).
//
// BSD 3-Clause License
//
// Copyright (c) 2018, The Regents of the University of California
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
///////////////////////////////////////////////////////////////////////////////

#include "Module.hpp"
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

void Module::setNetList(int NetId) {
  // Sets parameters given an entry in Nets file
  Netlist.push_back(NetId);
}
void Module::setParameterNodes(double width, double height) {
  // Sets parameters given an entry in Nodes file
  this -> xCoordinate = 0.0;
  this -> yCoordinate = 0.0;
  this -> width = width;
  this -> height = height;
  double points[][2] = {{0.0, 0.0}, {width, 0.0}, {width, height}, {0.0, height}};
  model::polygon< model::d2::point_xy<double> > tmp;
  append(tmp, points);
  boost::geometry::correct(tmp);
  this -> poly = tmp;
  boost::geometry::envelope(poly, envelope);
}

void Module::setParameterPl(double xCoordinate, double yCoordinate) {
  // Sets parameters given an entry in Pl file
  this -> setPos(xCoordinate, yCoordinate);
  initialX = xCoordinate;
  initialY = yCoordinate;
  this -> sigma = 50.0;
}

void Module::setPos(double x, double y) {
  // Sets position of a node object (lower-left corner)
  trans::translate_transformer<double, 2, 2> translate(x - this->xCoordinate, y - this->yCoordinate);
  model::polygon<model::d2::point_xy<double> > tmp;
  boost::geometry::transform(this->poly, tmp, translate);
  this->poly = tmp;
  updateCoordinates();
}

/**
upateCoordinates
Updates parameters of Node class from a geometry object
*/
void Module::updateCoordinates() {
  boost::geometry::model::d2::point_xy<double> centroid;
  boost::geometry::centroid(this->poly, centroid);
  this -> xBy2 = centroid.get<0>();
  this -> yBy2 = centroid.get<1>();

  boost::geometry::model::box< model::d2::point_xy<double> > envtmp;
  boost::geometry::envelope(this->poly, envtmp);
  this->envelope = envtmp;
  Point minCorner = this->envelope.min_corner();
  Point maxCorner = this->envelope.max_corner();
  this->width  = maxCorner.get<0>() - minCorner.get<0>();
  this->height = maxCorner.get<1>() - minCorner.get<1>();
  this->xCoordinate = bg::get<bg::min_corner, 0>(this->envelope);
  this->yCoordinate = bg::get<bg::min_corner, 1>(this->envelope);
}

/**
printExterior
print polygon vertices
*/
void Module::printExterior() const{
  for(auto it = boost::begin(boost::geometry::exterior_ring(this->poly)); it != boost::end(boost::geometry::exterior_ring(this->poly)); ++it) {
      double x = bg::get<0>(*it);
      double y = bg::get<1>(*it);
      cout << x << " " << y << endl;
  }
}

/**
printParameter
print node params
*/
void Module::printParameter() {
  cout << "name      " << this -> name << endl;
  cout << "Width         " << this -> width << endl;
  cout << "Height        " << this -> height << endl;
  cout << "X_Co-ordinate " << this -> xCoordinate << endl;
  cout << "Y_Co-ordinate " << this -> yCoordinate << endl;
  cout << "X/2           " << xBy2 << endl;
  cout << "Y/2           " << yBy2 << endl;
  cout << "NetList       ";
  vector < int > ::iterator it2;
  for (it2 = Netlist.begin(); it2 != Netlist.end(); ++it2) {
    cout << * it2 << " ";
  }
  this -> printExterior();
  cout << "\n" << endl;
}

void Module::init_module(int id, int lev, bool r) {
  root = r;
  idx = id;
  level = lev;
}
void Module::add_child(Module* m) {
  children.push_back(m);
}
void Module::insert_cell(int i) {
  cells.push_back(i);
}
bool Module::isleaf() {
  return leaf;
}
/*
void Hierarchy::init_hierarchy(int nl, vector <int> nm) {
  num_levels = nl;
  num_modules_per_layer = nm;

  for (int l=0; l<=num_levels; ++l) {
    Level tmp;
    tmp.level = l;
    levels.push_back(tmp);
  }
  root = create_hierarchy(0,0,true);
  root->root = true;
}

Module* Hierarchy::create_hierarchy(int id, int lev, bool r) {
  Module *m = new Module;
  m->init_module(id, lev, r);
  levels[lev].modules.push_back(m);

  if (num_levels <= lev) {
    m->leaf = true;
    return m;
  }

  int num_children = num_modules_per_layer[lev];
  for (int i =1; i<=num_children; ++i) {
    Module* mc;
    mc = create_hierarchy(i, lev+1, false);
    mc->parent = m;
    if(mc) {
      m->add_child(mc);
    }
  }
  return m;
}

void Hierarchy::print_param(int i) {
  int ii = 0;
  for (auto &lvl : levels) {
    if (i >= 0 && i == ii) {
      for (auto &m : lvl.modules) {
        cout << m->idx << " " << m->width << " " << m->height << endl;
      }
    }
    ii++ ;
  }
}

void Hierarchy::insert_cell(int cell_id, vector < int> cluster_id_vec, Module *m, int level) {
  if(num_levels <= level) {
    return;
  }
  if(cluster_id_vec.size() < 1) {
    // macro cell - just insert into every level
    vector < Module * > macroModulePath;
    for(int l=0; l<=num_levels; ++l) {
      Module *tmp = new Module;
      tmp->init_module(levels[l].modules.size() + 1, l, false);
      tmp->macroModule = true;
      tmp->fixed = 1;
      tmp->terminal = 1;
      tmp->insert_cell(cell_id);
      levels[l].modules.push_back(tmp);
      macroModulePath.push_back(tmp);
    }
    for(int l=1; l<num_levels; ++l) {
      Module *tmp = macroModulePath[l-1];
      if (l == 1) {
        root->children.push_back(tmp);
        tmp->parent = root;
      } else {
        tmp->parent = macroModulePath[l-2];
      }
      tmp->children.push_back(macroModulePath[l]);
    }
    macroModulePath[num_levels-1]->parent = macroModulePath[num_levels-2];
    macroModulePath[num_levels-1]->children.push_back(macroModulePath[num_levels]);
    macroModulePath[num_levels]->parent = macroModulePath[num_levels-1];
    macroModulePath[num_levels]->leaf = true;
  } else {
    int cluster_id = cluster_id_vec[level];
    m->children[cluster_id-1]->insert_cell(cell_id);
    insert_cell(cell_id, cluster_id_vec, m->children[cluster_id-1], level+1);
  }
}

Module *Hierarchy::get_leaf_module_from_id(int idx, Module *m) {
  if(m->leaf) {
    return m;
  } else {
    for (auto &child : m->children) {
      if(std::find(child->cells.begin(), child->cells.end(), idx) != child->cells.end()) {
        return get_leaf_module_from_id(idx, child);
      }
    }
  }
}

  Module *Hierarchy::get_level_module_from_id(int idx, int level, Module *m) {
  if(m->level == level) {
    return m;
  } else {
    for (auto &child : m->children) {
      if(std::find(child->cells.begin(), child->cells.end(), idx) != child->cells.end()) {
        return get_level_module_from_id(idx,level, child);
      }
    }
  }
}
*/
/**
for each level in the hierarchy, instantiate a netlist for that level:
associated nets for each module (list of net ids)
netlist-module mapping id -> list of modules
*/
/*void Hierarchy::set_netlist_hierarchy(map<int, vector<pPin> > netToCell) {
  int netidx = 1;
  for (auto &net : netToCell) {
    int netid = net.first;
    vector<pPin> pvec = net.second; //pvec_i.idx -> cell id
    vector<Module *> ms; // convert vector of pins to vector of clusters for that net add to netToModule in leaf-level
    for (auto &pin : pvec) {
      int cell_id = pin.idx;
      Module *m = get_leaf_module_from_id(cell_id, root);
      if(!m->leaf || !(std::find(m->cells.begin(), m->cells.end(), cell_id) != m->cells.end())) {
        cout << "[ERR] set_netlist_hierarchy: cell not found in leaf nodes: " << cell_id << " " << m->idx << " " << m->leaf << endl;
        return;
      }

      if(!(std::find(ms.begin(), ms.end(), m) != ms.end())) { // uniqueify module-level netlist
        m->setNetList(netidx);
        ms.push_back(m);
      }

    }
    levels.back().netToModule.insert(pair < int, vector < Module * > > (netidx, ms));
    netidx ++;
  }
  // now need to propagate up the hierarchy
  propagate_netlist(levels.back().level);
}

void Hierarchy::propagate_netlist(int lev) {
  if (lev <= 0) {
    return;
  }
  map<int, vector< Module * > >netToModules = levels[lev].netToModule;
  int netidx = 1;
  for (auto &net : netToModules) {
    int netid = net.first;
    vector<Module *> pvec = net.second; //pvec_i.idx -> cell id
    vector<Module *> ms; // convert vector of pins to vector of clusters for that net add to netToModule in leaf-level

    for (auto &module : pvec) {
      Module *m = module->parent;
      if(!(std::find(ms.begin(), ms.end(), m) != ms.end())) { // uniqueify module-level netlist
        m->setNetList(netidx); // crash here, module is root for some reason so parent m is garbage
        ms.push_back(m);
      }
    }
    if (ms.size() > 0 && lev > 0) {
      levels[lev-1].netToModule.insert(pair < int, vector < Module* > > (netidx, ms));
    }
    netidx ++;
  }
  propagate_netlist(lev - 1);
}

void Hierarchy::set_module_geometries(vector < Node > nodeId) { // should fix to be bottom up
  // first get leaf params
  int dim = 0;

  double area = 0.0;
  double xcenter = 0.0;
  double ycenter = 0.0;

  for (auto &m : levels.back().modules) {
    double xmin = 99999999999999.0;
    double ymin = 99999999999999.0;
    double xmax = 0.0;
    double ymax = 0.0;

    double area = 0.0;
    double xcenter = 0.0;
    double ycenter = 0.0;

    for (auto &cellid : m->cells) {
      double cellx = nodeId[cellid].xCoordinate;
      double celly = nodeId[cellid].yCoordinate;
      double cellw = nodeId[cellid].width;
      double cellh = nodeId[cellid].height;

      xmin = min(xmin, cellx);
      ymin = min(ymin, celly);
      xmax = max(xmax, cellx + cellw);
      ymax = max(ymax, celly + cellh);

      area += cellw * cellh;
      xcenter += cellx;
      ycenter += celly;
    }

    xcenter = xcenter / m->cells.size();
    ycenter = ycenter / m->cells.size();

    //cout << m->idx << " " << xmax - xmin<< " " << ymax - ymin << endl;
    //m->setParameterNodes(xmax - xmin, ymax - ymin);
    //m->setParameterPl(xmin, ymin);
    dim = ceil(sqrt(area));
    m->setParameterNodes(dim, dim);
    m->setParameterPl(xcenter, ycenter);
  }
  propagate_geometries(num_levels-1);
}

void Hierarchy::propagate_geometries(int lev) {
  if (lev <= 0) {
    return;
  }

  int dim = 0;
  vector <Module *> modules = levels[lev].modules;
  for (auto &module : modules) {
    double xmin = 99999999999999.0;
    double ymin = 99999999999999.0;
    double xmax = 0.0;
    double ymax = 0.0;

    double area = 0.0;
    double xcenter = 0.0;
    double ycenter = 0.0;

    for (auto &m : module->children) {
      double cellx = m->xCoordinate;
      double celly = m->yCoordinate;
      double cellw = m->width;
      double cellh = m->height;

      xmin = min(xmin, cellx);
      ymin = min(ymin, celly);
      xmax = max(xmax, cellx + cellw);
      ymax = max(ymax, celly + cellh);

      area += cellw * cellh;
      xcenter += cellx;
      ycenter += celly;
    }

    xcenter = xcenter / module->children.size();
    ycenter = ycenter / module->children.size();

    //module->setParameterNodes(xmax - xmin, ymax - ymin);
    //module->setParameterPl(xmin, ymin);

    dim = ceil(sqrt(area));
    module->setParameterNodes(dim, dim);
    module->setParameterPl(xcenter, ycenter);
  }
  propagate_geometries(lev - 1);
}

vector < Node >  Hierarchy::update_cell_positions_at_level(vector < Node > nodeId, int level) {
  for (auto &node : nodeId) {
    int cell_id = node.idx;
    Module *m = get_level_module_from_id(cell_id, level, root);
    if(!(std::find(m->cells.begin(), m->cells.end(), cell_id) != m->cells.end())) {
      cout << "[ERR] update_cell_positions_at_level: cell not found in leaf nodes: " << cell_id << " " << m->idx << " " << m->leaf << endl;
      return nodeId;
    }
    double mx = m->xCoordinate;
    double my = m->yCoordinate;
    double cx = node.initialX;
    double cy = node.initialY;
    double mx_orig = m->initialX;
    double my_orig = m->initialY;

    double trans_x = mx - mx_orig;
    double trans_y = my - my_orig;

    double new_cell_x = cx + trans_x;
    double new_cell_y = cy + trans_y;

    node.setPos(new_cell_x, new_cell_y);
  }
  return nodeId;
}

vector < Node >  Hierarchy::update_cell_positions(vector < Node > nodeId) {
  for (auto &node : nodeId) {
    int cell_id = node.idx;
    Module *m = get_leaf_module_from_id(cell_id, root);
    if(!m->leaf || !(std::find(m->cells.begin(), m->cells.end(), cell_id) != m->cells.end())) {
      cout << "[ERR] update_cell_positions: cell not found in leaf nodes: " << cell_id << " " << m->idx << " " << m->leaf << endl;
      return nodeId;
    }
    double mx = m->xCoordinate;
    double my = m->yCoordinate;
    double cx = node.initialX;
    double cy = node.initialY;
    double mx_orig = m->initialX;
    double my_orig = m->initialY;

    double trans_x = mx - mx_orig;
    double trans_y = my - my_orig;

    double new_cell_x = cx + trans_x;
    double new_cell_y = cy + trans_y;

    node.setPos(new_cell_x, new_cell_y);
  }
  return nodeId;
}*/


#include <boost/hana/fwd/maximum.hpp>


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/five.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check five behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::five;
  using boost::simd::Five;

  STF_TYPE_IS(decltype(Five<T>()), T);
  STF_EQUAL(Five<T>(), T(5));
  STF_EQUAL(five( as(T{}) ),T(5));
}

STF_CASE_TPL( "Check five behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::five;
  using boost::simd::Five;

  STF_TYPE_IS(decltype(Five<T>()), T);
  STF_IEEE_EQUAL(Five<T>(), T(5));
  STF_IEEE_EQUAL(five( as(T{}) ), T(5));
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_
#define KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_

#include <cmath>

#include <Eigen/Geometry>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationBase.hpp"
#include "kindr/rotations/eigen/RotationEigenFunctions.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class EulerAnglesXyz
 *  \brief Implementation of Euler angles (X-Y'-Z'' / roll-pitch-yaw) rotation based on Eigen::Matrix<Scalar,3,1>
 *
 *  The following typedefs are provided for convenience:
 *   - \ref eigen_impl::EulerAnglesXyzAD "EulerAnglesXyzAD" for active rotation and double primitive type
 *   - \ref eigen_impl::EulerAnglesXyzAF "EulerAnglesXyzAF" for active rotation and float primitive type
 *   - \ref eigen_impl::EulerAnglesXyzPD "EulerAnglesXyzPD" for passive rotation and double primitive type
 *   - \ref eigen_impl::EulerAnglesXyzPF "EulerAnglesXyzPF" for passive rotation and float primitive type
 *   - EulerAnglesRpyAD = EulerAnglesXyzAD
 *   - EulerAnglesRpyAF = EulerAnglesXyzAF
 *   - EulerAnglesRpyPD = EulerAnglesXyzPD
 *   - EulerAnglesRpyPF = EulerAnglesXyzPF
 *
 *  \tparam PrimType_ the primitive type of the data (double or float)
 *  \tparam Usage_ the rotation usage which is either active or passive
 *
 *  \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class EulerAnglesXyz : public EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_> {
 private:
  /*! \brief The base type.
   */
  typedef Eigen::Matrix<PrimType_, 3, 1> Base;

  /*! \brief vector of Euler angles [roll; pitch; yaw]
   */
  Base xyz_;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef Base Implementation;
  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  /*! \brief Rotation Vector as 3x1-matrix
   */
  typedef Base Vector;

  /*! \brief Default constructor using identity rotation.
   */
  EulerAnglesXyz()
    : xyz_(Base::Zero()) {
  }

  /*! \brief Constructor using three scalars.
   *  \param roll     first rotation angle around X axis
   *  \param pitch    second rotation angle around Y' axis
   *  \param yaw      third rotation angle around Z'' axis
   */
  EulerAnglesXyz(Scalar roll, Scalar pitch, Scalar yaw)
    : xyz_(roll,pitch,yaw) {
  }

  /*! \brief Constructor using Eigen::Matrix.
   *  \param other   Eigen::Matrix<PrimType_,3,1> [roll; pitch; yaw]
   */
  explicit EulerAnglesXyz(const Base& other)
  : xyz_(other) {
  }

  /*! \brief Constructor using another rotation.
   *  \param other   other rotation
   */
  template<typename OtherDerived_>
  inline explicit EulerAnglesXyz(const RotationBase<OtherDerived_, Usage_>& other)
    : xyz_(internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation()) {
  }

  /*! \brief Assignment operator using another rotation.
   *  \param other   other rotation
   *  \returns referece
   */
  template<typename OtherDerived_>
  EulerAnglesXyz& operator =(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Parenthesis operator to convert from another rotation.
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_>
  EulerAnglesXyz& operator ()(const RotationBase<OtherDerived_, Usage_>& other) {
    this->toImplementation() = internal::ConversionTraits<EulerAnglesXyz, OtherDerived_>::convert(other.derived()).toImplementation();
    return *this;
  }

  /*! \brief Returns the inverse of the rotation.
   *  \returns the inverse of the rotation
   */
  EulerAnglesXyz inverted() const {
    return EulerAnglesXyz(eigen_internal::getInverseRpy<PrimType_, PrimType_>(this->toImplementation()));
  }

  /*! \brief Inverts the rotation.
   *  \returns reference
   */
  EulerAnglesXyz& invert() {
    *this = this->inverted();
    return *this;
  }

  /*! \brief Returns the Euler angles in a vector.
   *  \returns  vector Eigen::Matrix<Scalar,3, 1>
   */
  inline const Vector vector() const {
    return this->toImplementation();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline Base& toImplementation() {
    return static_cast<Base&>(xyz_);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation for direct manipulation (recommended only for advanced users)
   */
  inline const Base& toImplementation() const {
    return static_cast<const Base&>(xyz_);
  }

  /*! \brief Returns roll (X) angle.
   *  \returns roll angle (scalar)
   */
  inline Scalar roll() const {
    return xyz_(0);
  }

  /*! \brief Returns pitch (Y') angle.
   *  \returns pitch angle (scalar)
   */
  inline Scalar pitch() const {
    return xyz_(1);
  }

  /*! \brief Returns yaw (Z'') angle.
   *  \returns yaw angle (scalar)
   */
  inline Scalar yaw() const {
    return xyz_(2);
  }

  /*! \brief Sets roll (X) angle.
   */
  inline void setRoll(Scalar roll) {
    xyz_(0) = roll;
  }

  /*! \brief Sets pitch (Y') angle.
   */
  inline void setPitch(Scalar pitch) {
    xyz_(1) = pitch;
  }

  /*! \brief Sets yaw (Z'') angle.
   */
  inline void setYaw(Scalar yaw) {
    xyz_(2) = yaw;
  }

  /*! \brief Gets roll (X) angle.
   *  \returns roll angle (scalar)
   */
  inline Scalar x() const {
    return xyz_(0);
  }

  /*! \brief Gets pitch (Y') angle.
   *  \returns pitch angle (scalar)
   */
  inline Scalar y() const {
    return xyz_(1);
  }

  /*! \brief Gets yaw (Z'') angle.
   *  \returns yaw angle (scalar)
   */
  inline Scalar z() const {
    return xyz_(2);
  }

  /*! \brief Sets roll (X) angle.
   */
  inline void setX(Scalar x) {
    xyz_(0) = x;
  }

  /*! \brief Sets pitch (Y') angle.
   */
  inline void setY(Scalar y) {
    xyz_(1) = y;
  }

  /*! \brief Sets yaw (Z'') angle.
   */
  inline void setZ(Scalar z) {
    xyz_(2) = z;
  }

  /*! \brief Sets the rotation to identity.
   *  \returns reference
   */
  EulerAnglesXyz& setIdentity() {
    xyz_.setZero();
    return *this;
  }

  /*! \brief Returns a unique Euler angles rotation with angles in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
   *  This function is used to compare different rotations.
   *  \returns copy of the Euler angles rotation which is unique
   */
  EulerAnglesXyz getUnique() const {
    Base xyz(kindr::common::floatingPointModulo(x()+M_PI,2*M_PI)-M_PI,
             kindr::common::floatingPointModulo(y()+M_PI,2*M_PI)-M_PI,
             kindr::common::floatingPointModulo(z()+M_PI,2*M_PI)-M_PI); // wrap all angles into [-pi,pi)

    const double tol = 1e-3;

    // wrap angles into [-pi,pi),[-pi/2,pi/2),[-pi,pi)
    if(xyz.y() < -M_PI/2 - tol)
    {
      if(xyz.x() < 0) {
        xyz.x() = xyz.x() + M_PI;
      } else {
        xyz.x() = xyz.x() - M_PI;
      }

      xyz.y() = -(xyz.y() + M_PI);

      if(xyz.z() < 0) {
        xyz.z() = xyz.z() + M_PI;
      } else {
        xyz.z() = xyz.z() - M_PI;
      }
    }
    else if(-M_PI/2 - tol <= xyz.y() && xyz.y() <= -M_PI/2 + tol)
    {
      xyz.x() -= xyz.z();
      xyz.z() = 0;
    }
    else if(-M_PI/2 + tol < xyz.y() && xyz.y() < M_PI/2 - tol)
    {
      // ok
    }
    else if(M_PI/2 - tol <= xyz.y() && xyz.y() <= M_PI/2 + tol)
    {
      // todo: M_PI/2 should not be in range, other formula?
      xyz.x() += xyz.z();
      xyz.z() = 0;
    }
    else // M_PI/2 + tol < xyz.y()
    {
      if(xyz.x() < 0) {
        xyz.x() = xyz.x() + M_PI;
      } else {
        xyz.x() = xyz.x() - M_PI;
      }

      xyz.y() = -(xyz.y() - M_PI);

      if(xyz.z() < 0) {
        xyz.z() = xyz.z() + M_PI;
      } else {
        xyz.z() = xyz.z() - M_PI;
      }
    }

    return EulerAnglesXyz(xyz);
  }

  /*! \brief Modifies the Euler angles rotation such that the angles lie in [-pi,pi),[-pi/2,pi/2),[-pi,pi).
   *  \returns reference
   */
  EulerAnglesXyz& setUnique() {
    *this = getUnique();
    return *this;
  }

  /*! \brief Concenation operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator*.
   *  \returns the concenation of two rotations
   */
  using EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_>::operator*;

  /*! \brief Equivalence operator.
   *  This is explicitly specified, because Eigen::Matrix provides also an operator==.
   *  \returns true if two rotations are similar.
   */
  using EulerAnglesXyzBase<EulerAnglesXyz<PrimType_, Usage_>, Usage_>::operator==;

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const EulerAnglesXyz& xyz) {
    out << xyz.toImplementation().transpose();
    return out;
  }
};

//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesXyz<double, RotationUsage::ACTIVE>  EulerAnglesXyzAD;
//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesXyz<float,  RotationUsage::ACTIVE>  EulerAnglesXyzAF;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesXyz<double, RotationUsage::PASSIVE> EulerAnglesXyzPD;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesXyz<float,  RotationUsage::PASSIVE> EulerAnglesXyzPF;

//! \brief Equivalent Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) class
template <typename PrimType_, enum RotationUsage Usage_>
using EulerAnglesRpy = EulerAnglesXyz<PrimType_, Usage_>;

//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesRpy<double, RotationUsage::ACTIVE>  EulerAnglesRpyAD;
//! \brief Active Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesRpy<float,  RotationUsage::ACTIVE>  EulerAnglesRpyAF;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with double primitive type
typedef EulerAnglesRpy<double, RotationUsage::PASSIVE> EulerAnglesRpyPD;
//! \brief Passive Euler angles rotation (X,Y',Z'' / roll,pitch,yaw) with float primitive type
typedef EulerAnglesRpy<float,  RotationUsage::PASSIVE> EulerAnglesRpyPF;

} // namespace eigen_impl

namespace internal {

template<typename PrimType_, enum RotationUsage Usage_>
class get_scalar<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>> {
 public:
  typedef PrimType_ Scalar;
};

template<typename PrimType_, enum RotationUsage Usage_>
class get_matrix3X<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>>{
 public:
  typedef int  IndexType;

  template <IndexType Cols>
  using Matrix3X = Eigen::Matrix<PrimType_, 3, Cols>;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>> {
 public:
  typedef eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE> OtherUsage;
};

template<typename PrimType_>
class get_other_usage<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>> {
 public:
  typedef eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE> OtherUsage;
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::AngleAxis<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::AngleAxis<SourcePrimType_, Usage_>& aa) {
//    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::getRpyFromAngleAxis<SourcePrimType_, DestPrimType_>(aa.toImplementation()));
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::RotationQuaternion<DestPrimType_, Usage_>(aa));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationVector<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationVector<SourcePrimType_, Usage_>& rotationVector) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::AngleAxis<SourcePrimType_, Usage_>(rotationVector));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<SourcePrimType_, Usage_>& q) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromQuaternion<SourcePrimType_, DestPrimType_>(q.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::RotationMatrix<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::RotationMatrix<SourcePrimType_, Usage_>& R) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromRotationMatrix<SourcePrimType_, DestPrimType_>(R.toImplementation()));
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<SourcePrimType_, Usage_>& xyz) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(xyz.toImplementation().template cast<DestPrimType_>());
  }
};

template<typename DestPrimType_, typename SourcePrimType_, enum RotationUsage Usage_>
class ConversionTraits<eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>, eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>> {
 public:
  inline static eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<SourcePrimType_, Usage_>& zyx) {
    return eigen_impl::EulerAnglesXyz<DestPrimType_, Usage_>(eigen_impl::eigen_internal::getRpyFromYpr<SourcePrimType_, DestPrimType_>(zyx.toImplementation()));
  }
};

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Multiplication Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

//template<typename PrimType_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>, RotationUsage::ACTIVE>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>, RotationUsage::ACTIVE>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE> mult(const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>& a, const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::ACTIVE>(eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::ACTIVE>(b).toImplementation()));
//  }
//};
//
//template<typename PrimType_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>, RotationUsage::PASSIVE>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>, RotationUsage::PASSIVE>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE> mult(const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>& a, const eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, RotationUsage::PASSIVE>(eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, RotationUsage::PASSIVE>(b).toImplementation()));
//  }
//};

//template<typename PrimType_, enum RotationUsage Usage_>
//class MultiplicationTraits<RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>, Usage_>, RotationBase<eigen_impl::EulerAnglesXyz<PrimType_, Usage_>, Usage_>> {
// public:
//  inline static eigen_impl::EulerAnglesXyz<PrimType_, Usage_> mult(const eigen_impl::EulerAnglesXyz<PrimType_, Usage_>& a, const eigen_impl::EulerAnglesXyz<PrimType_, Usage_>& b) {
//    return eigen_impl::EulerAnglesXyz<PrimType_, Usage_>(eigen_impl::RotationQuaternion<PrimType_, Usage_>(
//                                                                 eigen_impl::RotationQuaternion<PrimType_, Usage_>(a).toImplementation()*
//                                                                 eigen_impl::RotationQuaternion<PrimType_, Usage_>(b).toImplementation()));
//  }
//};
//

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Rotation Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Comparison Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_EULERANGLESXYZ_HPP_ */


/*
 libs/numeric/odeint/examples/stochastic_euler.hpp

 Copyright 2012 Karsten Ahnert
 Copyright 2012-2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 Stochastic euler stepper example and Ornstein-Uhlenbeck process

 Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

typedef boost::array< double , 1 > state_type;

using namespace boost::numeric::odeint;

//[ generation_functions_own_steppers
class custom_stepper
{
public:

    typedef double value_type;
    // ...
};

class custom_controller
{
    // ...
};

class custom_dense_output
{
    // ...
};
//]

//[ generation_functions_get_controller
namespace boost { namespace numeric { namespace odeint {

template<>
struct get_controller< custom_stepper >
{
    typedef custom_controller type;
};

} } }
//]

//[ generation_functions_controller_factory
namespace boost { namespace numeric { namespace odeint {

template<>
struct controller_factory< custom_stepper , custom_controller >
{
    custom_controller operator()( double abs_tol , double rel_tol , const custom_stepper & ) const
    {
        return custom_controller();
    }

    custom_controller operator()( double abs_tol , double rel_tol , double max_dt ,
                                  const custom_stepper & ) const
    {
        // version with maximal allowed step size max_dt
        return custom_controller();
    }
};

} } }
//]

int main( int argc , char **argv )
{
    {
        typedef runge_kutta_dopri5< state_type > stepper_type;

        /*
        //[ generation_functions_syntax_auto
        auto stepper1 = make_controlled( 1.0e-6 , 1.0e-6 , stepper_type() );
        // or with max step size limit:
        // auto stepper1 = make_controlled( 1.0e-6 , 1.0e-6 , 0.01, stepper_type() );

        auto stepper2 = make_dense_output( 1.0e-6 , 1.0e-6 , stepper_type() );
        //]
        */

        //[ generation_functions_syntax_result_of
        boost::numeric::odeint::result_of::make_controlled< stepper_type >::type stepper3 = make_controlled( 1.0e-6 , 1.0e-6 , stepper_type() );
        (void)stepper3;
        boost::numeric::odeint::result_of::make_dense_output< stepper_type >::type stepper4 = make_dense_output( 1.0e-6 , 1.0e-6 , stepper_type() );
        (void)stepper4;
        //]
    }

    {
        /*
        //[ generation_functions_example_custom_controller
        auto stepper5 = make_controlled( 1.0e-6 , 1.0e-6 , custom_stepper() );
        //]
        */

        boost::numeric::odeint::result_of::make_controlled< custom_stepper >::type stepper5 = make_controlled( 1.0e-6 , 1.0e-6 , custom_stepper() );
        (void)stepper5;
    }
    return 0;
}


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

#include <cloe/utility/evaluate.hpp>

#include <algorithm>   // for less_equal, ...
#include <functional>  // for function<>, bind
#include <string>      // for string

#include <boost/lexical_cast.hpp>  // for lexical_cast<>

namespace cloe {
namespace utility {

std::function<bool(double)> compile_evaluation(const std::string& s) {
  std::string op;
  size_t pos = 0;
  for (auto ch : s) {
    if ((ch >= '0' && ch <= '9') || ch == '.') {
      break;
    }
    pos++;
    if (ch == ' ') {
      continue;
    }
    op.push_back(ch);
  }
  double num = boost::lexical_cast<double>(s.substr(pos));  // NOLINT
  return compile_evaluation(op, num);
}

std::function<bool(double)> compile_evaluation(const std::string& op, double num) {
  if (op == "==") {
    return std::bind(std::equal_to<double>(), std::placeholders::_1, num);
  } else if (op == "!=") {
    return std::bind(std::not_equal_to<double>(), std::placeholders::_1, num);
  } else if (op == "<") {
    return std::bind(std::less<double>(), std::placeholders::_1, num);
  } else if (op == "<=") {
    return std::bind(std::less_equal<double>(), std::placeholders::_1, num);
  } else if (op == ">") {
    return std::bind(std::greater<double>(), std::placeholders::_1, num);
  } else if (op == ">=") {
    return std::bind(std::greater_equal<double>(), std::placeholders::_1, num);
  } else {
    throw std::out_of_range("unknown operator '" + op + "'");
  }
}

}  // namespace utility
}  // namespace cloe


#pragma once

#include <map>
#include <memory>
#include <boost/optional.hpp>

#include <crab/common/types.hpp>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-compare"

namespace crab {

 namespace domains {

   namespace term {
      typedef int term_id;
      typedef int var_id;

      enum term_kind { TERM_CONST, TERM_VAR, TERM_APP };

      template<class Num, class Ftor>
      class term : public writeable {
       public:
        typedef term<Num, Ftor> term_t;
        typedef std::shared_ptr<term_t> term_ptr;

        virtual term_kind kind(void) = 0;

        bool operator<(term_t& other);

        virtual void write(crab_os& o) = 0;

        int depth;
      };

      template<class Num, class Ftor>
      class term_ref {
       public:
        typedef term<Num, Ftor> term_t;
        typedef std::shared_ptr<term_t> term_ptr;

        term_ref(term_ptr _p)
            : p(_p)
        { }

        term_ref(term_t* _p)
            : p(_p)
        { }

        bool operator<(const term_ref& other) const
        {
          return *p < *(other.p);
        }

        term_ptr p;
      };

      // Dispatch for different kinds of terms.
      template<class Num, class Ftor>
      class const_term : public term<Num, Ftor> {
       public:
        const_term(Num _val)
            : val(_val)
        { }
        term_kind kind(void) { return TERM_CONST; }

        void write(crab_os& o)
        {
          o << "c(" << val << ")";
        }
        Num val;
      };

      template<class Num, class Ftor>
      class var_term : public term<Num, Ftor> {
       public:
        var_term(var_id _var)
            : var(_var)
        { }
        term_kind kind(void) { return TERM_VAR; }
        void write(crab_os& o)
        {
          o << "v(" << var << ")";
        }
        var_id var;
      };

      template<class Num, class Ftor>
      class ftor_term : public term<Num, Ftor> {
       public:
        ftor_term(Ftor _f, std::vector<term_id>& _args)
            : ftor(_f), args(_args)
        { }
        term_kind kind(void) { return TERM_APP; }
        void write(crab_os& o)
        {
          //o << "<op>(";
          o << ftor << "(";
          bool first = true;
          for(term_id c : args)
          {
            if(first)
              first = false;
            else
              o << ", ";
            o << c;
          }
          o << ")";
        }

        Ftor ftor;
        std::vector<term_id> args;
      };

      // These functions are super-unsafe.
      template<class Num, class Ftor>
      var_id term_var(term<Num, Ftor>* term)
      {
        return static_cast< var_term<Num, Ftor>* >(term)->var;
      }
      template<class Num, class Ftor>
      Num term_const(term<Num, Ftor>* term)
      {
        return static_cast< const_term<Num, Ftor>* >(term)->val;
      }

      template<class Num, class Ftor>
      Ftor term_ftor(term<Num, Ftor>* term)
      {
        return static_cast< ftor_term<Num, Ftor>* >(term)->ftor;
      }

      template<class Num, class Ftor>
      std::vector<term_id>& term_args(term<Num, Ftor>* term)
      {
        return static_cast< ftor_term<Num, Ftor>* >(term)->args;
      }

      // Term ordering
      template<class Num, class Ftor>
      bool term<Num, Ftor>::operator<(term_t& other)
      {
        if(kind() != other.kind())
          return kind() < other.kind();

        switch(kind())
        {
          case TERM_CONST:
            {
              return term_const(this) < term_const(&other);
            }
            break;
          case TERM_VAR:
            {
              return term_var(this) < term_var(&other);
            }
          case TERM_APP:
            {
              if(term_ftor(this) != term_ftor(&other))
                return term_ftor(this) < term_ftor(&other);

              std::vector<int>& xargs(term_args(this));
              std::vector<int>& yargs(term_args(&other));

              if(xargs.size() != yargs.size())
                return xargs.size() < yargs.size();

              for(unsigned int ii = 0; ii< xargs.size(); ii++)
              {
                if(xargs[ii] != yargs[ii])
                  return xargs[ii] < yargs[ii];
              }
              return false;
            }
          default:
            CRAB_ERROR("Unsupported term kind.");
        }
      }

      template<typename TermTable>
      class congruence_closure_solver;

      template<class Num, class Ftor>
      class term_table : public writeable {
       public:
        typedef term_table<Num, Ftor> term_table_t;
        typedef term<Num, Ftor> term_t;
        typedef std::shared_ptr<term_t> term_ptr;
        typedef term_ref<Num, Ftor> term_ref_t;
        typedef term_id term_id_t;

        typedef const_term<Num, Ftor> const_term_t;
        typedef var_term<Num, Ftor> var_term_t;
        typedef ftor_term<Num, Ftor> ftor_term_t;

        // For establishing a mapping between tables
        typedef std::map<term_id, term_id> term_map_t;
        typedef std::map<std::pair<term_id, term_id>, term_id> gener_map_t;

        term_table(void)
            : free_var(0)
        { }

        term_table(const term_table_t& o)
            : free_var(o.free_var), _map(o._map),
              terms(o.terms), _parents(o._parents), _ref_count(o._ref_count),
              _depth(o._depth),
              free_terms(o.free_terms)
        { }

        term_table_t& operator=(const term_table_t& o)
        {
          free_var = o.free_var;
          _map = o._map;
          terms = o.terms;
          _ref_count = o._ref_count;
          _parents = o._parents;
          _depth = o._depth;
          free_terms = o.free_terms;
          return *this;
        }

        term_id make_const(const Num& n) {
          term_ref_t ref(new const_term_t(n));
          return add_term(ref);
        };
        boost::optional<term_id> find_const(Num& n) {
          term_ref_t ref(new const_term_t(n));
          return find_term(ref);
        }

        term_id make_var(var_id& n) {
          free_var = std::max(free_var, n+1);
          term_ref_t ref(new var_term_t(n));
          return add_term(ref);
        };

        term_id fresh_var(void) {
          var_id v = free_var++;
          return make_var(v);
        }

        template <typename T, typename ... Types>
        void collect_args(std::vector<T>& vec, T& x, Types ... rest)
        {
          vec.push_back(x); collect_args(vec, rest...);
        }

        template <typename T>
        void collect_args(std::vector<T>& vec) { }

        term_id apply_ftor(const Ftor& f, std::vector<term_id>& ids)
        {
          term_ref_t ref(new ftor_term_t(f, ids));
          return add_term(ref);
        }

        template <typename ... Types>
        term_id apply_ftor(const Ftor& f, Types ... args)
        {
          std::vector<term_id> ids;
          collect_args(ids, args...);
          return apply_ftor(f, ids);
        }

        boost::optional<term_id> find_ftor(Ftor& f, std::vector<term_id>& ids)
        {
          term_ref_t ref(new ftor_term_t(f, ids));
          return find_term(ref);
        }

        template<typename ... Types>
        boost::optional<term_id> find_ftor(Ftor& f, Types ... args)
        {
          std::vector<term_id> ids;
          collect_args(ids, args...);
          return find_ftor(f, ids);
        }

        term_t* get_term_ptr(term_id t)
        {
          return terms[t].p.get();
        }

        void add_ref(term_id t)
        {
          _ref_count[t]++;
        }

        void deref(term_id t, std::vector<term_id>& forgotten)
        {
          assert(_ref_count[t]);
          _ref_count[t]--;
          if(!_ref_count[t])
          {
            forgotten.push_back(t);
            free_terms.push_back(t);
            term_ref_t ref(terms[t]);
            _map.erase(ref);
            if(ref.p.get()->kind() == TERM_APP)
            {
              for(term_id c : term_args(ref.p.get()))
                deref(c, forgotten);
            }
          }
        }

        // Check if a tx is a generalization of ty, given an existing context.
        bool map_leq(term_table_t& y, term_id tx, term_id ty, term_map_t& map)
        {
          auto it = map.find(ty);
          if(it != map.end())
            return (*it).second == tx;

          term_ref_t ry(y.terms[ty]);
          term_ref_t rx(terms[tx]);
          switch(ry.p.get()->kind())
          {
            case TERM_CONST:
              {
                if(rx.p.get()->kind() != TERM_CONST || term_const(rx.p.get()) != term_const(ry.p.get()))
                  return false;
              }
              break;
            case TERM_APP:
              {
                if(rx.p.get()->kind() != TERM_APP || term_ftor(rx.p.get()) != term_ftor(ry.p.get()))
                  return false;

                std::vector<int>& xargs(term_args(rx.p.get()));
                std::vector<int>& yargs(term_args(ry.p.get()));

                if(xargs.size() != yargs.size())
                  return false;

                for(unsigned int ii = 0; ii< xargs.size(); ii++)
                {
                  if(!map_leq(y, xargs[ii], yargs[ii], map))
                    return false;
                }
              }
              break;
            case TERM_VAR:
              break;
          }
          map[ty] = tx;
          return true;
        }

        term_id_t generalize(term_table_t& y, term_id tx, term_id ty,
                             term_table_t& out, gener_map_t& g_map)
        {
          auto txy = std::make_pair(tx, ty);
          auto it = g_map.find(txy);
          if(it != g_map.end())
          {
            return (*it).second;
        } else {
            // Haven't found this pair yet.
            // Generalize the arguments.
            auto px(terms[tx].p.get());
            auto py(y.terms[ty].p.get());

            term_kind kx = px->kind();
            term_kind ky = py->kind();

            // If either is a variable, we just create a fresh variable
            term_id_t ret;
            do
            {
              if(kx == TERM_VAR || ky == TERM_VAR || kx != ky)
              {
                ret = out.fresh_var(); break;
              }

              if(kx == TERM_CONST)
              {
                if(term_const(px) != term_const(py))
                {
                  ret = out.fresh_var();
                } else {
                  ret = out.make_const(term_const(px));
                }
              } else {
                assert(kx == TERM_APP);
                if(term_ftor(px) != term_ftor(py))
                {
                  ret = out.fresh_var(); break;
                }

                std::vector<term_id>& xargs(term_args(px));
                std::vector<term_id>& yargs(term_args(py));
                if(xargs.size() != yargs.size())
                {
                  ret = out.fresh_var(); break;
                }

                std::vector<term_id> xyargs;
                for(unsigned int ii = 0; ii < xargs.size(); ii++)
                  xyargs.push_back(generalize(y, xargs[ii], yargs[ii], out, g_map));
                ret = out.apply_ftor(term_ftor(px), xyargs);
              }
            } while(0);

            g_map[txy] = ret;
            return ret;
          }
        }

        // Copy term x from table tx
        term_id_t copy_term (term_table_t& tx, term_id x, term_map_t& ren_map) {
          auto it = ren_map.find(x);
          if(it != ren_map.end()) {
            return (*it).second;
          } else {
            auto px(tx.terms[x].p.get());
            term_kind kx = px->kind();
            term_id_t ret;
            if(kx == TERM_VAR) {
              ret = fresh_var();
            } else if(kx == TERM_CONST) {
              ret = make_const(term_const(px));
            } else {
              assert(kx == TERM_APP);
              std::vector<term_id>& xargs(term_args(px));
              std::vector<term_id> yargs;
              for(unsigned int ii = 0; ii < xargs.size(); ii++)
                yargs.push_back (copy_term (tx, xargs[ii], ren_map));
              ret = apply_ftor( term_ftor(px), yargs);
            }

            ren_map[x] = ret;
            return ret;
          }
        }

        std::vector<term_id_t>& parents(term_id_t id)
        {
          return _parents[id];
        }

        void write(crab_os& o) {
          bool first = true;
          for(unsigned int ti = 0; ti < terms.size(); ti++)
          {
            if(first)
              first = false;
            else
              o << ", ";
            term_t* p(terms[ti].p.get());
            o << ti << " -> " << *p;
          }
        }

        int size() { return terms.size(); }

        int depth(term_id t) { return _depth[t]; }

       protected:
        boost::optional<term_id> find_term(term_ref_t ref)
        {
          auto it = _map.find(ref);
          if(it != _map.end())
            return boost::optional<term_id>(it->second);
          else
            return boost::optional<term_id>();
        }

        // When we know that a germ doesn't already exist.
        term_id fresh_term(term_ref_t ref)
        {
          if(free_terms.size() > 0)
          {
            term_id t = free_terms.back();
            free_terms.pop_back();
            terms[t] = ref;
            _parents[t].clear();
            _depth[t] = 0;
            _ref_count[t] = 0;
            return t;
          } else {
            term_id t = terms.size();
            terms.push_back(ref);
            _ref_count.push_back(0);
            _parents.push_back(std::vector<term_id>());
            _depth.push_back(0);
            return t;
          }
        }

        term_id add_term(term_ref_t ref)
        {
          auto it = _map.find(ref);
          if(it != _map.end())
          {
            return (*it).second;
          } else {
            term_id id = fresh_term(ref);
            _map[ref] = id;
            if(ref.p.get()->kind() == TERM_APP)
            {
              unsigned int c_depth = 0;
              for(term_id c : term_args(ref.p.get()))
              {
                assert(c < _ref_count.size());
                _ref_count[c] += 1;
                _parents[c].push_back(id);
                c_depth = std::max(c_depth, _depth[c]);
              }
              _depth[id] = 1+c_depth;
            }
              /* Not true, as we're garbage collecting terms */
//            assert(_map.size() == id+1);
            return id;
          }
        }

        int free_var;
        std::map<term_ref_t, term_id> _map;
        std::vector<term_ref_t> terms;
        std::vector< std::vector<term_id_t> > _parents;
        std::vector<unsigned int> _ref_count;
        std::vector<unsigned int> _depth;
        std::vector<term_id> free_terms;
      };

      template<typename TermTable>
      class congruence_closure_solver : public boost::noncopyable {

       public:

        typedef typename TermTable::term_id_t term_id_t;
        typedef typename TermTable::term_t term_t;
        typedef boost::container::flat_set< term_id_t > term_set_t;
        typedef std::map <term_id_t, term_set_t> ccpar_map_t;
        typedef std::map <term_id_t, term_id_t> find_map_t;
        typedef std::pair<term_id_t, term_id_t> equation_t;
        typedef std::map <term_id_t, std::vector<term_id_t> > member_map_t;

       private:
        TermTable* _ttbl;
        ccpar_map_t _ccpar_map;
        find_map_t _find_map;
        std::vector<equation_t> _eqs;
        term_set_t _terms;
        member_map_t _members;

       public:

        congruence_closure_solver (TermTable* ttbl): _ttbl (ttbl) { }

        void operator+=(equation_t eq) {
          _terms.insert (eq.first);
          _terms.insert (eq.second);
          _eqs.push_back (eq);
        }

        void run () {
          for (auto eq: _eqs) {
            term_id_t x = eq.first;
            term_id_t y = eq.second;
            merge (x, y);
          }
        }

        void run (std::vector<equation_t>& eqs) {
          for (auto e: eqs) { *this += e; }
          run ();
        }

        // return the equivalence class associated with t
        term_id_t get_class (term_id_t t) {
          return find (t);
        }

        // return the members of an equivalence class of k
        std::vector<term_id_t>& get_members (term_id_t t) {
          auto it = _members.find (t);
          if (it != _members.end ())  {
            return it->second;
          } else {
            t = find (t); // find representative of the t's class
            std::vector<term_id_t> members;
            for (auto x: _terms) {
              if (find (x) == t /*find (t)*/)
                members.push_back (x);
            }
            auto res = _members.insert(std::make_pair (t, members));
            return (res.first)->second;
          }
        }

        // return the size of the equivalence class of k
        size_t get_size (term_id_t t) {
          return get_members (t).size ();
        }

        void write (crab_os&o) {
          for (auto t1: _terms) {
            o << "t" << t1 << " --> {";
            for (auto t2: _terms) {
              if (find (t1) == find (t2))
                o << "t" << t2 << ";";
            }
            o << "}\n";
          }
        }

        std::vector<term_id_t>& get_parents (term_id_t t){
          return _ttbl->parents (t);
        }

       private:

        term_set_t& get_ccpar (term_id_t t) {
          auto it = _ccpar_map.find (t);
          if (it == _ccpar_map.end ()) {
            auto res = _ccpar_map.insert (std::make_pair (t, term_set_t ()));
            return (res.first)->second;
          } else {
            return it->second;
          }
        }

        // return the representative of i
        term_id_t find (term_id_t t) {
          auto it = _find_map.find (t);
          if (it == _find_map.end ()) {
            _find_map.insert (std::make_pair (t,t));
            return t;
          }

          if (it->second == t) {
            return it->second;
          } else {
            return find (it->second);
          }
        }

        // i2 is the new representative
        void do_union (term_id_t t1, term_id_t t2) {
          _find_map [t1] = _find_map [t2];

          term_set_t& ccpar1 = get_ccpar (t1);
          term_set_t& ccpar2 = get_ccpar (t2);
          ccpar2.insert (ccpar1.begin(), ccpar1.end());

          term_t* t1_ptr = _ttbl->get_term_ptr(t1);
          assert (t1_ptr);
          if (t1_ptr->kind () == TERM_APP) {
            std::vector<term_id_t>& par1 = get_parents (t1);
            ccpar2.insert (par1.begin (), par1.end());
          }

          term_t* t2_ptr = _ttbl->get_term_ptr(t2);
          assert (t2_ptr);
          if (t2_ptr->kind () == TERM_APP) {
            std::vector<term_id_t>& par2 = get_parents (t2);
            ccpar2.insert (par2.begin (), par2.end());
          }
          ccpar1.clear ();
        }

        // pre: t1 and t2 are TERM_APP
        bool is_congruent (term_id_t t1, term_id_t t2) {

          term_t* t1_ptr = _ttbl->get_term_ptr(t1);
          term_t* t2_ptr = _ttbl->get_term_ptr(t2);

          assert (t1_ptr->kind() == TERM_APP);
          assert (t2_ptr->kind() == TERM_APP);

          if (term_ftor (t1_ptr) != term_ftor (t2_ptr))
           return false;

          std::vector<term_id_t>& xargs(term_args(t1_ptr));
          std::vector<term_id_t>& yargs(term_args(t2_ptr));

         if (xargs.size () != yargs.size ())
           return false;

         for (unsigned i=0;i < xargs.size() ; i++) {
           if (find (xargs [i]) != find (yargs [i]))
             return false;
         }
         return true;
        }

        void merge (term_id_t t1, term_id_t t2) {

          if (find (t1) == find (t2)) return;

          do_union (t1, t2);
          auto &ps1 = get_ccpar (t1);
          auto &ps2 = get_ccpar (t2);
          for (auto p1 : ps1) {
            for (auto p2 : ps2) {
              if ((find (p1) != find (p2)) && is_congruent (p1,p2) )
                merge (p1,p2);
            }
          }
        }
      }; // end congruence_closure_solver

   } //end namespace term
 } // end namepsace domains
} // end namespace crab
#pragma GCC diagnostic pop


#ifndef GAME_HPP
#define GAME_HPP

#include "Game/Food.hpp"
#include "Game/Snake.hpp"
#include <Eigen/Dense>
#include <vector>

/*
 * Used to generate seeds
 */
namespace Seeder {
    extern std::mt19937 generator;
    extern std::uniform_int_distribution<int> seeds;
    extern unsigned int getSeed();
}; // namespace Seeder

/*
 * Abstract class to handle games
 */
class Game {
private:
    // Members for RNG
    unsigned int seed; // seed for reproducing results
    std::mt19937 gen;
    std::uniform_int_distribution<int> randX;
    std::uniform_int_distribution<int> randY;
    std::uniform_int_distribution<int> randDir;

protected:
    // Grid and NN
    void init(const std::vector<Eigen::MatrixXd> &w = {}, const std::vector<Eigen::VectorXd> &b = {});

public:
    FoodManager foodManager;            // for food generation
    Snake snake;                        // Snake
    std::vector<std::vector<int>> grid; // 2D game grid
    int foodCount;                      // count of food on grid

    Game();
    Game(unsigned int);
    Game(const std::vector<Eigen::MatrixXd> &w, const std::vector<Eigen::VectorXd> &b);
    Game(const std::vector<Eigen::MatrixXd> &w, const std::vector<Eigen::VectorXd> &b, unsigned int);
    Game(const Game &);            // copy constructor - copies seed too
    Game &operator=(const Game &); // copy constructor - copies seed too

    // Functions for RNG
    int getX();
    int getY();
    int getDir();
    std::mt19937 &getGen();
    unsigned int getSeed();
    void reseed();      // to reseed the game
    void initObjects(); // initializes snake and food
    void saveToFile(int);
    void loadFromFile(int);

    virtual void start() = 0; // abstract function to start the game
};

#endif


#include <boost/math/distributions/chi_squared.hpp>
#include <Eigen/Eigen>
//#include <ros/ros.h>

//using boost::math::chi_squared;
//using boost::math::cdf;

namespace perception_oru
{

template <typename PointT>
void AdaptiveOctTree<PointT>::setParameters(bool _useDornikHansen,
        bool _useFlatness,
        double _RSS_THRESHOLD,
        double _DH_SIGNIFICANCE_LVL,
        double _MIN_CELL_SIZE,
        double _FLAT_FACTOR,
        double _BIG_CELL_SIZE,
        double _SMALL_CELL_SIZE
                                           )
{

    this->BIG_CELL_SIZE = _BIG_CELL_SIZE;
    this->SMALL_CELL_SIZE = _SMALL_CELL_SIZE;

    useDornikHansen      = _useDornikHansen;
    useFlatness          = _useFlatness;
    RSS_THRESHOLD        = _RSS_THRESHOLD;
    DH_SIGNIFICANCE_LVL  = _DH_SIGNIFICANCE_LVL;
    MIN_CELL_SIZE        = _MIN_CELL_SIZE;
    FLAT_FACTOR          = _FLAT_FACTOR;

    parametersSet = true;
}
/** empty! default constructor
  */
template <typename PointT>
AdaptiveOctTree<PointT>::AdaptiveOctTree() : OctTree<PointT>()
{
    if(!parametersSet)
    {
        setParameters();
    }
}

/** constructor, calls parent OctTree constructor
  */
template <typename PointT>
AdaptiveOctTree<PointT>::AdaptiveOctTree(pcl::PointXYZ center, double xsize, double ysize,
        double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent, unsigned int _depth) :
    OctTree<PointT>(center,xsize,ysize,zsize,type,_parent,_depth)
{
    if(!parametersSet)
    {
        setParameters();
    }
}

/** empty destructor, all data are deallocated by parent class
  */
template <typename PointT>
AdaptiveOctTree<PointT>::~AdaptiveOctTree()
{
}

/**
  finds all leafs of this tree and fills the vector of pointers to the leafs
  */
template <typename PointT>
void AdaptiveOctTree<PointT>::computeTreeLeafs()
{
    if(this->isLeaf())
    {
        myTreeLeafs.push_back(this);
        return;
    }

    myTreeLeafs.clear();
    std::vector<OctTree<PointT>*> next;
    next.push_back(this);

    while(next.size()>0)
    {
        OctTree<PointT> *cur = next.front();
        if(cur!=NULL)
        {
            if(cur->isLeaf())
            {
                myTreeLeafs.push_back(cur);
            }
            else
            {
                for(int i=0; i<8; i++)
                {
                    OctTree<PointT>* tmp = cur->getChild(i);
                    if(tmp!=NULL)
                    {
                        next.push_back(tmp);
                    }
                }
            }
        }
        next.erase(next.begin());
    }
}

/**
go through all leafs and split the ones with high residuals
  */
template <typename PointT>
void AdaptiveOctTree<PointT>::postProcessPoints()
{

    //compute leafs as OctTree*
    computeTreeLeafs();
    //cout<<"leafs :"<<myTreeLeafs.size()<<endl;

    for(unsigned int i=0; i<myTreeLeafs.size(); i++)
    {
        NDTCell<PointT> * nd = dynamic_cast<NDTCell<PointT>*>((myTreeLeafs[i])->myCell_);
        if(nd == NULL) continue;
        if(useDornikHansen)
        {
            double significance = computeDornikHansen(nd);
            if(significance < 0)
            {
                //there were not enough points, let's clear the cell!
                //(myTreeLeafs[i])->myCell->points.clear();
                continue;
            }
            if(significance < DH_SIGNIFICANCE_LVL)
            {
                //split the leafs
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }
        }
        else if(useFlatness)
        {
            nd->computeGaussian();
            if(!nd->hasGaussian_)
            {
                continue;
            }

            Eigen::Vector3d evals = nd->getEvals();
            int idMin, idMax;
            double minEval = evals.minCoeff(&idMin);
            double maxEval = evals.maxCoeff(&idMax);
            int idMiddle = -1;
            for(int j=0; j<3; j++)
            {
                if(j!=idMin && j!=idMax)
                {
                    idMiddle =  j;
                }
            }
            if(idMiddle < 0) continue;

            if(minEval*FLAT_FACTOR > evals(idMiddle))
            {
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }

        }
        else
        {
            double rss = computeResidualSquare(nd);
            if(rss > RSS_THRESHOLD)
            {
                //split the leafs
                std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
                myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
            }
        }
    }

    this->leafsCached_ = false;
}

/**
  performs the Dornik-Hansen Omnibus normality test
*/
template <typename PointT>
double AdaptiveOctTree<PointT>::computeDornikHansen(NDTCell<PointT> *cell)
{
    double pval = 1;
    double Ep = 0;
    //test statistics breaks down for n<=7
    if(cell->points_.size() <= 7) return -1;
    cell->computeGaussian();

    //degree is 2*number_of_dimensions
    boost::math::chi_squared dist(6);

    Eigen::Vector3d mean = cell->getMean();
    Eigen::Matrix3d C = cell->getCov();

    Eigen::MatrixXd Xhat(cell->points_.size(),3); //, tmpMat;

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {
        Xhat(i,0) = cell->points_[i].x - mean(0);
        Xhat(i,1) = cell->points_[i].y - mean(1);
        Xhat(i,2) = cell->points_[i].z - mean(2);
    }

    //Compute transform for observed points
    Eigen::Matrix3d V;
    Eigen::Matrix3d H;
    Eigen::Vector3d lambda;
    Eigen::Matrix3d L;
    Eigen::Matrix3d R1;

    Eigen::MatrixXd R;

    V(0,0) = 1/sqrt(C(0,0));
    V(1,1) = 1/sqrt(C(1,1));
    V(2,2) = 1/sqrt(C(2,2));

    C = V*C*V;
    Eigen::EigenSolver<Eigen::Matrix3d> eig(C);
    H = eig.eigenvectors().real();
    lambda = eig.eigenvalues().real();

    //covariance is not positive semidefinate
    if(lambda.minCoeff() <= 0) return -1;

    L(0,0) = 1/sqrt(lambda(0));
    L(1,1) = 1/sqrt(lambda(1));
    L(2,2) = 1/sqrt(lambda(2));

    //transform observations
    R1 = H*L*H.transpose()*V;
    R = R1*Xhat.transpose();

    //compute skewness and kurtois of new observations (in each dimension)
    //samples are zero mean, compute for each dimension standard deviation
    Ep = 0.0;
    double n = R.cols();
    for(unsigned int dim = 0; dim < 3; dim ++)
    {
        double m2 = 0, m3 = 0, m4 =0;
        double b1 = 0, b2;
        double beta, omega2, gamma, y;
        double gamma2, a,c, k, alpha, chi;
        double z1,z2;

        for(unsigned int i=0; i<R.cols(); i++)
        {
            m2 += pow(R(dim,i),2);
            m3 += pow(R(dim,i),3);
            m4 += pow(R(dim,i),4);
        }
        m2 /= R.cols();
        m3 /= R.cols();
        m4 /= R.cols();
//	cout <<"dim "<<dim<<" m2 "<<m2<<" m3 "<<m3<<" m4 "<<m4<<endl;
        b1 = m3/(pow(m2,1.5));
        b2 = m4/(pow(m2,2));

//	cout<<"b1 "<<b1<<" b2 "<<b2<<endl;

        //compute Z1 and Z2
        beta = 3*(n*n + 27*n -70)*(n+1)*(n+3)/((n-2)*(n+5)*(n+7)*(n+9));
        omega2 = -1+sqrt(2*(beta-1));
        gamma = 1/sqrt(log(sqrt(omega2)));
        y = b1*sqrt((omega2-1)*(n+1)*(n+3)/(12*(n-2)));
        z1 = gamma*log(y+sqrt(y*y+1));

        gamma2 = (n-3)*(n+1)*(n*n+15*n-4);
        a = (n-2)*(n+5)*(n+7)*(n*n+27*n-70)/(6*gamma2);
        c = (n-7)*(n+5)*(n+7)*(n*n+2*n-5)/(6*gamma2);
        k = (n+5)*(n+7)*(pow(n,3)+37*n*n+11*n-313)/(12*gamma2);
        alpha = a + b1*b1*c;
        chi = (b2-1-b1*b1)*2*k;
        z2 = (pow((chi/(2*alpha)),(1./3.))-1+(1./(9*alpha)))*sqrt(9*alpha);

//	cout<<"z1: "<<z1<<" z2: "<<z2<<endl;

        //compute Ep
        Ep += z1*z1 + z2*z2;
    }

    //compute probability from chi square cdf
    pval = 1-boost::math::cdf(dist,Ep);

//    cout<<"Ep "<<Ep<<endl;
//    cout<<"P: "<<pval<<endl;
    if(pval>DH_SIGNIFICANCE_LVL)
    {
        double dx,dy,dz;
        cell->getDimensions(dx,dy,dz);
        std::cout<<"final split was at ("<<dx<<","<<dy<<","<<dz<<"); pval is "<<pval<<std::endl;
    }
    return 0;

}
/**
fits a 3d gaussian in the cell and computes the residual squares sum
*/
template <typename PointT>
double AdaptiveOctTree<PointT>::computeResidualSquare(NDTCell<PointT> *cell)
{
    double rss = 0; //residual sum squared
    double meanResidual = 0;
    double residualVar = 0;
    if(cell->points_.size() <= 3) return 0;

    cell->computeGaussian();

    Eigen::Vector3d cur, curProd;
    Eigen::Matrix3d cov = cell->getCov();
    Eigen::Vector3d mean = cell->getMean();
    //Eigen::LLT<Eigen::Matrix3d> lltOfCov = cov.llt();

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {

        cur(0) = cell->points_[i].x - mean(0);
        cur(1) = cell->points_[i].y - mean(1);
        cur(2) = cell->points_[i].z - mean(2);

        //lltOfCov.solve(cur,&curProd);
        //rss += cur.dot(curProd);
        rss += cur.dot(cur);
        meanResidual += cur.norm()/cell->points_.size();
    }

    for(unsigned int i=0; i< cell->points_.size(); i++)
    {
        cur(0) = cell->points_[i].x - mean(0);
        cur(1) = cell->points_[i].y - mean(1);
        cur(2) = cell->points_[i].z - mean(2);

        residualVar += pow(cur.norm()-meanResidual,2)/(cell->points_.size()-1);
    }
    double bic = rss/residualVar + log(cell->points_.size());
//    ROS_INFO("rss %lf mean %lf var %lf bic %lf",rss,meanResidual,residualVar,bic);
    return bic;
}

/**
  splits a cell and returns a vector of the newly created children
  iterates points downwards
  */
template <typename PointT>
std::vector<OctTree<PointT>*> AdaptiveOctTree<PointT>::splitTree(OctTree<PointT> *octLeaf)
{
    std::vector<OctTree<PointT>*> newLeafs;

    if(octLeaf->isLeaf())
    {
        double xs,ys,zs;
        octLeaf->myCell_->getDimensions(xs,ys,zs);

        double cellSize = (xs+ys+zs)/3.; //average for now

        if(octLeaf->depth_ > this->MAX_DEPTH || cellSize <= this->MIN_CELL_SIZE )
        {
            //just store point, we can't split any more
            return newLeafs;
        }

        pcl::PointXYZ myCenter = octLeaf->myCell_->getCenter();

        //branch leaf
        for(unsigned int it=0; it<8; it++)
        {

            pcl::PointXYZ newCenter;

            //computes the center of the it'th child
            newCenter.x = (myCenter.x + pow(-1.,it/4)*xs/4.);
            newCenter.y = (myCenter.y + pow(-1.,it/2)*ys/4.);
            newCenter.z = (myCenter.z + pow(-1.,(it+1)/2)*zs/4.);

            octLeaf->children_[it] = new OctTree<PointT>(newCenter,xs/2,ys/2,
                    zs/2, octLeaf->myCell_, this, this->depth_+1);
            newLeafs.push_back(octLeaf->children_[it]);
        }
        //add current points
        for(unsigned int jt=0; jt<octLeaf->myCell_->points_.size(); jt++)
        {
            size_t ind = octLeaf->getIndexForPoint(octLeaf->myCell_->points_[jt]);
            octLeaf->children_[ind]->addPoint(octLeaf->myCell_->points_[jt]);
        }
        octLeaf->leaf_=false;
        octLeaf->myCell_->points_.clear();
    }

    return newLeafs;
}

/**
  creates an oct tree with the same parameters.
  \note the points are not copied in teh returned instance
  */
template <typename PointT>
SpatialIndex<PointT>* AdaptiveOctTree<PointT>::clone()
{
    if(this->myCell_ == NULL)
    {
        return new AdaptiveOctTree();
    }
    double sx,sy,sz;
    this->myCell_->getDimensions(sx,sy,sz);
    AdaptiveOctTree<PointT> *tr = new AdaptiveOctTree<PointT>(this->myCell_->getCenter(),sx,sy,sz,this->myCell_);
    return tr;
}

}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SIMD_ACSC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_ACSC_HPP_INCLUDED

#include <boost/simd/function/scalar/acsc.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/acsc.hpp>

#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include <QTime>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include <sensor_msgs/JointState.h>

double AbsEncoder[13];
double IncEncoder[13];

void abs_enc_receive(const  sensor_msgs::JointState &msg)
{
    for (int i = 0; i < 13; ++i) {
        AbsEncoder[i]=msg.position[i];
    }
}

void inc_enc_receive(const  sensor_msgs::JointState &msg)
{
    for (int i = 0; i < 13; ++i) {
       IncEncoder[i]=msg.position[i];
    }
}

int main(int argc, char *argv[])
{

    QByteArray data;

    QTime pc_time;
    QString name;
    name="/home/cast/Desktop/deflection_data_save/data_";
    name+=QString::number(pc_time.currentTime().hour())+"_";
    name+=QString::number(pc_time.currentTime().minute())+"_";
    name+=QString::number(pc_time.currentTime().second())+"_";
    name+=".txt";
    QFile myfile(name);

    vector<double> cntrl(13);
    QCgenerator QC;
    //check _timesteps

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
//    ros::Subscriber abs_enc = nh.subscribe("surena/abs_joint_state", 100,abs_enc_receive );
//    ros::Subscriber inc_enc = nh.subscribe("surena/inc_joint_state", 100,inc_enc_receive );

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    double dt=.005;
    double time;

    double  d_theta=dt*M_PI/2/10;
    double hip_roll_angle=0;
    bool increase=true;
    while (ros::ok())
    {
        ROS_INFO("hip_roll_angle=%f",hip_roll_angle);

        if(increase){hip_roll_angle+=d_theta;}
        if(time<2){hip_roll_angle=0;}
        if(hip_roll_angle>M_PI/9){hip_roll_angle=M_PI/9;}
//        else{hip_roll_angle-=d_theta;}
//        if(hip_roll_angle>M_PI/3 || hip_roll_angle<0){increase=!increase;}

        //commands in radian
        cntrl[0]=0.0;
        cntrl[1]=0.0;//RHY
        cntrl[2]=0.0-hip_roll_angle;//RHR(<0 to avoid collision)
        cntrl[3]=0.0;//RHP
        cntrl[4]=0.0;//RK
        cntrl[5]=0.0;//RAP
        cntrl[6]=0.0;//RAR
        cntrl[7]=0.0;//LHY
        cntrl[8]=0.0;//LHR(>0 to avoid collision)
        cntrl[9]=0.0;//LHP
        cntrl[10]=0.0;//LK
        cntrl[11]=0.0;//LAP
        cntrl[12]=0.0;//LAR

    vector<int> qref(12);
    qref=QC.ctrldata2qc(cntrl);

msg.data.clear();
    for(int  i = 0;i < 12;i++)
    {
        msg.data.push_back(qref[i]);
        //msg.data.push_back(0);
    }

    chatter_pub.publish(msg);
    //  ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);
    time+=dt;
//    data.append(QString::number(time)+","+QString::number(hip_roll_angle)+","+QString::number(AbsEncoder[9+1])+","+QString::number(IncEncoder[9+1])+"\n");
//    myfile.open(QFile::ReadWrite);
//    myfile.write(data);
//    myfile.close();

    ros::spinOnce();
    loop_rate.sleep();

}

    return 0;
}


// Copyright (c) 2015-2019, FlightAware LLC.
// Copyright (c) 2015, Oliver Jowett <oliver@mutability.co.uk>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <iomanip>
#include <iostream>

#include "crc.h"

#include <boost/preprocessor/repetition/enum.hpp>

namespace crc {
    namespace detail {
        // generates the CRC table at compile time (!)
        const std::uint32_t crc_polynomial = 0xfff409U;

        template <std::uint32_t c, int k = 8> struct crcgen : crcgen<((c & 0x00800000) ? crc_polynomial : 0) ^ (c << 1), k - 1> {};

        template <std::uint32_t c> struct crcgen<c, 0> {
            enum { value = (c & 0x00FFFFFF) };
        };

#define CRCGEN(Z, N, _) detail::crcgen<N << 16>::value
        std::uint32_t crc_table[256] = {BOOST_PP_ENUM(256, CRCGEN, _)};
#undef CRCGEN

        std::map<std::uint32_t, unsigned> syndromes_short;
        std::map<std::uint32_t, unsigned> syndromes_long;

        void init_syndromes() {
            syndromes_short.clear();
            std::vector<std::uint8_t> message_short(56 / 8);
            for (unsigned i = 5; i < 56; ++i) {
                message_short[i / 8] ^= 1 << (7 - (i & 7));
                syndromes_short[message_residual(message_short)] = i;
                message_short[i / 8] ^= 1 << (7 - (i & 7));
            }

            syndromes_long.clear();
            std::vector<std::uint8_t> message_long(112 / 8);
            for (unsigned i = 5; i < 112; ++i) {
                message_long[i / 8] ^= 1 << (7 - (i & 7));
                syndromes_long[message_residual(message_long)] = i;
                message_long[i / 8] ^= 1 << (7 - (i & 7));
            }
        }
    } // namespace detail
};    // namespace crc


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

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

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

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

/*! \file test/analyticlgmswaptionengine.hpp
    \brief analytic LGM swaption engine test
*/

#ifndef quantext_test_analyticlgmswaptionengine_hpp
#define quantext_test_analyticlgmswaptionengine_hpp

#include <boost/test/unit_test.hpp>

namespace testsuite {

//! AnalyticLgmSwaptionEngine tests
class AnalyticLgmSwaptionEngineTest {
public:
    /*! Test if the fixed coupon adjustment in the analytic LGM swaption engine accounting for a dual curve setup is
     * zero in a mono curve setup. */
    static void testMonoCurve();

    /*! Test if the fixed coupon adjustment in the analytic LGM swaption engine accounting for a dual curve setup is
     * within a range of expected values for a given spread in a dual curve setup. */
    static void testDualCurve();

    /*! Test the result of the analytic LGM swaption engine against those of the Gaussian1dSwaptionEngine and
     * FdHullWhiteSwaptionEngine. */
    static void testAgainstOtherEngines();

    /*! Test whether the price of an european swaption in the analytic LGM swaption engine is invariant under the LGM
     * model invariances (scaling and shifting). */
    static void testLgmInvariances();

    static boost::unit_test_framework::test_suite* suite();
};
} // namespace testsuite

#endif