//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_RATIONAL_HPP
#define BOOST_MATH_TOOLS_RATIONAL_HPP

#include <boost/array.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/mpl/int.hpp>

#if BOOST_MATH_POLY_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_POLY_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/polynomial_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif
#if BOOST_MATH_RATIONAL_METHOD == 1
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner1_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 2
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner2_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#elif BOOST_MATH_RATIONAL_METHOD == 3
#  define BOOST_HEADER() <BOOST_JOIN(boost/math/tools/detail/rational_horner3_, BOOST_MATH_MAX_POLY_ORDER).hpp>
#  include BOOST_HEADER()
#  undef BOOST_HEADER
#endif
namespace boost{ namespace math{ namespace tools{

//
// Forward declaration to keep two phase lookup happy:
//
template <class T, class U>
U evaluate_polynomial(const T* poly, U const& z, std::size_t count);

namespace detail{

template <class T, class V, class Tag>
inline V evaluate_polynomial_c_imp(const T* a, const V& val, const Tag*)
{
   return evaluate_polynomial(a, val, Tag::value);
}

} // namespace detail

//
// Polynomial evaluation with runtime size.
// This requires a for-loop which may be more expensive than
// the loop expanded versions above:
//
template <class T, class U>
inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count)
{
   BOOST_ASSERT(count > 0);
   U sum = static_cast<U>(poly[count - 1]);
   for(int i = static_cast<int>(count) - 2; i >= 0; --i)
   {
      sum *= z;
      sum += static_cast<U>(poly[i]);
   }
   return sum;
}
//
// Compile time sized polynomials, just inline forwarders to the
// implementations above:
//
template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const T(&a)[N], const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a), val, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_polynomial(const boost::array<T,N>& a, const V& val)
{
   typedef mpl::int_<N> tag_type;
   return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()), val, static_cast<tag_type const*>(0));
}
//
// Even polynomials are trivial: just square the argument!
//
template <class T, class U>
inline U evaluate_even_polynomial(const T* poly, U z, std::size_t count)
{
   return evaluate_polynomial(poly, z*z, count);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const T(&a)[N], const V& z)
{
   return evaluate_polynomial(a, z*z);
}

template <std::size_t N, class T, class V>
inline V evaluate_even_polynomial(const boost::array<T,N>& a, const V& z)
{
   return evaluate_polynomial(a, z*z);
}
//
// Odd polynomials come next:
//
template <class T, class U>
inline U evaluate_odd_polynomial(const T* poly, U z, std::size_t count)
{
   return poly[0] + z * evaluate_polynomial(poly+1, z*z, count-1);
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const T(&a)[N], const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a) + 1, z*z, static_cast<tag_type const*>(0));
}

template <std::size_t N, class T, class V>
inline V evaluate_odd_polynomial(const boost::array<T,N>& a, const V& z)
{
   typedef mpl::int_<N-1> tag_type;
   return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()) + 1, z*z, static_cast<tag_type const*>(0));
}

template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count);

namespace detail{

template <class T, class U, class V, class Tag>
inline V evaluate_rational_c_imp(const T* num, const U* denom, const V& z, const Tag*)
{
   return boost::math::tools::evaluate_rational(num, denom, z, Tag::value);
}

}
//
// Rational functions: numerator and denominator must be
// equal in size.  These always have a for-loop and so may be less
// efficient than evaluating a pair of polynomials. However, there
// are some tricks we can use to prevent overflow that might otherwise
// occur in polynomial evaluation, if z is large.  This is important
// in our Lanczos code for example.
//
template <class T, class U, class V>
V evaluate_rational(const T* num, const U* denom, const V& z_, std::size_t count)
{
   V z(z_);
   V s1, s2;
   if(z <= 1)
   {
      s1 = static_cast<V>(num[count-1]);
      s2 = static_cast<V>(denom[count-1]);
      for(int i = (int)count - 2; i >= 0; --i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   else
   {
      z = 1 / z;
      s1 = static_cast<V>(num[0]);
      s2 = static_cast<V>(denom[0]);
      for(unsigned i = 1; i < count; ++i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   return s1 / s2;
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const T(&a)[N], const U(&b)[N], const V& z)
{
   return detail::evaluate_rational_c_imp(a, b, z, static_cast<const mpl::int_<N>*>(0));
}

template <std::size_t N, class T, class U, class V>
inline V evaluate_rational(const boost::array<T,N>& a, const boost::array<U,N>& b, const V& z)
{
   return detail::evaluate_rational_c_imp(a.data(), b.data(), z, static_cast<mpl::int_<N>*>(0));
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_RATIONAL_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_SQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/arithmetic/functions/sqrt.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/touint.hpp>
#include <boost/simd/include/functions/simd/is_gez.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm256_sqrt_pd(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm256_sqrt_ps(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<uint64_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::touint(boost::simd::sqrt(boost::simd::tofloat(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_, boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<int64_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      return boost::simd::toint(boost::simd::sqrt(boost::simd::tofloat(a0)));
    }
  };
} } }
#endif
#endif


#include "humanoids/humanoids.hpp"
#include "humanoids/hull2d.hpp"
#include "sco/expr_op_overloads.hpp"
#include "sco/expr_vec_ops.hpp"
#include "sco/modeling_utils.hpp"
#include "osgviewer/osgviewer.hpp"
#include "trajopt/common.hpp"
#include "trajopt/collision_avoidance.hpp"
#include "trajopt/kinematic_terms.hpp"
#include "trajopt/plot_callback.hpp"
#include "trajopt/problem_description.hpp"
#include "trajopt/configuration_space.hpp"
#include "utils/eigen_conversions.hpp"
#include "utils/stl_to_string.hpp"
#include "utils/vector_ops.hpp"
#include <boost/assign.hpp>
#include <boost/foreach.hpp>
#include <openrave-core.h>
#include <openrave/openrave.h>
using namespace boost::assign;
using namespace Eigen;
using namespace OpenRAVE;
using namespace std;
using namespace trajopt;
using namespace util;

bool gEnablePlot = true;
const float DIST_PEN=.02;
const float COLL_COEFF=10;
const float VEL_COEFF = .25;
string data_dir() {
  string out = DATA_DIR;
  return out;
}

/**
import openravepy
env = openravepy.Environment()
env.Load("gfe.xml")
robot = env.GetRobots()[0]
rfoot = robot.GetLink("r_foot")
aabb = rfoot.ComputeLocalAABB(rfoot)
aabb.pos()+aabb.extents()
aabb.pos()-aabb.extents()
 */
MatrixX2d GetLocalAabbPoly() {
  MatrixX2d out(4,2);
  out << 0.17939, 0.062707,
      0.17939, -0.061646,
      -0.08246, -0.061646,
      -0.08246, 0.061646;
  return out;
}
MatrixX2d local_aabb_poly = GetLocalAabbPoly();

MatrixX2d GetLeftPoly(const RobotBase& gfe) {
  OpenRAVE::Vector v = gfe.GetLink("l_foot")->GetTransform().trans;
  v.z = 0;
  return local_aabb_poly.rowwise() + Vector2d(v.x, v.y).transpose();
}
MatrixX2d GetRightPoly(const RobotBase& gfe) {
  OpenRAVE::Vector v = gfe.GetLink("r_foot")->GetTransform().trans;
  v.z = 0;
  return local_aabb_poly.rowwise() + Vector2d(v.x, v.y).transpose();
}
template <typename MatrixT>
MatrixT concat0(const MatrixT& x, const MatrixT& y) {
  MatrixT out(x.rows() + y.rows(), x.cols());
  out.topRows(x.rows()) = x;
  out.bottomRows(y.rows()) = y;
  return out;
}
template <typename MatrixT>
void extend0(MatrixT& x, const MatrixT& y) {
  x.conservativeResize(x.rows()+y.rows(), NoChange);
  x.bottomRows(y.rows()) = y;
}

MatrixX2d GetBothPoly(const RobotBase& gfe) {
  MatrixX2d left = GetLeftPoly(gfe);
  MatrixX2d right = GetRightPoly(gfe);
  MatrixX2d verts = concat0(left, right);
  MatrixX2d hull = hull2d(verts);
  return hull;
}

void SetLeftZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetLeftPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetRightZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetRightPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetBothZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetBothPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetLinkFixed(TrajOptProb& prob, KinBody::LinkPtr link, int t, const OpenRAVE::Transform& tlink) {
//  BoolVec allbutz(6,true);
//  allbutz[5] = false;
  prob.addConstraint(ConstraintPtr(new CartPoseConstraint(prob.GetVarRow(t), tlink, prob.GetRAD(), link)));
}
void SetLinkFixed(TrajOptProb& prob, KinBody::LinkPtr link, int t) {
  OpenRAVE::Transform tlink = link->GetTransform();
  SetLinkFixed(prob, link, t, tlink);
}
void SetStartFixed(TrajOptProb& prob) {
  DblVec cur_dofvals = prob.GetRAD()->GetDOFValues();
  VarArray& vars = prob.GetVars();
  for (int j=0; j < vars.cols(); ++j) {
    prob.addLinearConstraint(exprSub(AffExpr(vars(0,j)), cur_dofvals[j]), EQ);
  }
}

TrajArray Optimize(TrajOptProbPtr prob) {
  // optimize
  BasicTrustRegionSQP opt(prob);
  opt.improve_ratio_threshold_ = .1;
//  opt.min
  TrajArray init(prob->GetNumSteps(), prob->GetNumDOF());
  DblVec cur_dofvals = prob->GetRAD()->GetDOFValues();
  for (int i=0; i < prob->GetNumSteps(); ++i) init.row(i) = toVectorXd(cur_dofvals);
  init += DblMatrix::Random(init.rows(), init.cols()) * .01;
  opt.initialize(DblVec(init.data(), init.data()+init.rows()*init.cols()));
  if (gEnablePlot) opt.addCallback(PlotCallback(*prob));
  opt.optimize();
  return getTraj(opt.x(), prob->GetVars());
}

void LeftRightStep(RobotBasePtr gfe, TrajArray& out) {
  OpenRAVE::KinBody::LinkPtr lfoot = gfe->GetLink("l_foot"),
      rfoot = gfe->GetLink("r_foot");

  CollisionCheckerPtr cc = CollisionChecker::GetOrCreate(*gfe->GetEnv());
  vector<Collision> collisions;
  cc->AllVsAll(collisions);
//  cc->IgnoreLink(lfoot);
//  cc->IgnoreLink(rfoot);

  // determine foot placement pose
  // set up optimization problem

  int n_steps = 5;
  float step_dist = .3;

  RobotAndDOFPtr rad(new RobotAndDOF(gfe, arange(gfe->GetDOF()), OR::DOF_Transform));

  BoolVec xonly(6, false);
  xonly[3] = true;

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));

    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
//      prob->addConstr(ConstraintPtr(new CartPoseConstraint(prob->GetVarRow(i), Tfoottarg, rad, rfoot, xonly)));
      SetLinkFixed(*prob, lfoot, i);
      SetLeftZMP(*prob, i);
    }
    OpenRAVE::Transform Tfootnow = rfoot->GetTransform();
    OpenRAVE::Transform Tfoottarg = Tfootnow;
    Tfoottarg.trans.x = lfoot->GetTransform().trans.x + step_dist; // 30 cm forward
//    if (Tfoottarg.trans.x >= -.1 && Tfoottarg.trans.x < .1) Tfoottarg.trans.z += .05;
    SetLinkFixed(*prob, rfoot, n_steps-1, Tfoottarg);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, lfoot, i);
      SetLinkFixed(*prob, rfoot, i);
      SetBothZMP(*prob, i);
    }
    SetRightZMP(*prob, n_steps-1);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, rfoot, i);
      SetRightZMP(*prob, i);
    }
    OpenRAVE::Transform Tfootnow = lfoot->GetTransform();
    OpenRAVE::Transform Tfoottarg = Tfootnow;
    Tfoottarg.trans.x = rfoot->GetTransform().trans.x + step_dist; // 30 cm forward
    SetLinkFixed(*prob, lfoot, n_steps-1, Tfoottarg);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, lfoot, i);
      SetLinkFixed(*prob, rfoot, i);
      SetBothZMP(*prob, i);
    }
    SetLeftZMP(*prob, n_steps-1);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

}

void AnimateTrajectory(RobotAndDOFPtr rad, TrajArray& traj, OSGViewerPtr viewer) {
  for (int i=0; i < traj.rows(); ++i) {
    cout << "step " << i << endl;
    rad->SetDOFValues(toDblVec(traj.row(i)));
    viewer->Idle();
  }
}

int main(int argc, char* argv[]) {
  RaveInitialize(false);
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(getenv("HOME")) + "/Proj/darpa-proposal/drclogs.env.xml");
  env->Load(string(getenv("HOME")) + "/Proj/drc/gfe.xml");
  vector<RobotBasePtr> robots; env->GetRobots(robots);
  RobotBasePtr gfe = robots[0];
  OSGViewerPtr viewer(new OSGViewer(env));
  env->AddViewer(viewer);

  RobotAndDOFPtr rad(new RobotAndDOF(gfe, arange(gfe->GetDOF()), OR::DOF_Transform));

  // translate to starting position
  {
    OpenRAVE::Transform T;
    T.trans.y += 1;
    T.trans.z = 0.92712;
    T.trans.x -= .35;
    gfe->SetTransform(T);
  }

  TrajArray traj(0, rad->GetDOF());
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);

//  AnimateTrajectory(rad, traj, viewer);

  {
    gfe->SetDOFValues(DblVec(gfe->GetDOF(), 0));
    OpenRAVE::Transform T = gfe->GetTransform();
    T.rot = OR::Vector(1,0,0,.2);
    T.rot.normalize4();
    T.trans = OR::Vector(2.4, 1.4, .92712);
    gfe->SetTransform(T);
    int n_steps = 5;
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      SetLinkFixed(*prob, gfe->GetLink("l_foot"), i);
      SetLinkFixed(*prob, gfe->GetLink("r_foot"), i);
      SetLeftZMP(*prob, i);
      SetRightZMP(*prob, i);

//      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
//      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
//      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
    }
    OpenRAVE::Transform buttonpose = env->GetKinBody("bigredbutton")->GetTransform();
    buttonpose.trans.z += .1;
    BoolVec justposition(6, false);
    justposition[3] = justposition[4] = justposition[5] = true;
    prob->addConstraint(ConstraintPtr(new CartPoseConstraint(prob->GetVarRow(n_steps-1), buttonpose,
        rad, gfe->GetLink("r_hand"), justposition)));
    TrajArray traj1 = Optimize(prob);
    extend0(traj, traj1);
  }

AnimateTrajectory(rad, traj, viewer);

  viewer->Idle();
ofstream trajfile("/tmp/traj.txt");
trajfile << traj << endl;

  RaveDestroy();
}


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * 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.
 *
 */
// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>
//for libxc
#include <votca/xtp/votca_config.h>

#include <votca/xtp/numerical_integrations.h>
#include <boost/math/constants/constants.hpp>
#include <votca/xtp/radial_euler_maclaurin_rule.h>
#include <votca/xtp/sphere_lebedev_rule.h>
#include <votca/xtp/aoshell.h>
#include <votca/tools/constants.h>
#include <numeric>

#include <votca/xtp/aomatrix.h>
#include <fstream>
#include <boost/timer/timer.hpp>
#include <boost/algorithm/string.hpp>

#include <iterator>
#include <string>

namespace votca {
    namespace xtp {
        namespace ub = boost::numeric::ublas;

        double NumericalIntegration::getExactExchange(const string _functional){
#ifdef LIBXC

            double exactexchange=0.0;
            Vxc_Functionals map;
            std::vector<string> strs;

            boost::split(strs, _functional, boost::is_any_of(" "));
            if (strs.size()>2 ) {
                throw std::runtime_error("Too many functional names");
            }
            else if (strs.size()<1 ) {
                throw std::runtime_error("Specify at least one functional");
            }

            for (unsigned i=0;i<strs.size();i++){

                int func_id = map.getID(strs[i]);
                if (func_id<0){
                    exactexchange=0.0;
                    break;
                }
                xc_func_type func;
                if (xc_func_init(&func, func_id, XC_UNPOLARIZED) != 0) {
                    fprintf(stderr, "Functional '%d' not found\n", func_id);
                    exit(1);
                }
                if (exactexchange>0 && func.cam_alpha>0){
                    throw std::runtime_error("You have specified two functionals with exact exchange");
                }
                exactexchange+=func.cam_alpha;

            }
            return exactexchange;

#else
            return 0.0;
#endif

        }

        ub::matrix<double> NumericalIntegration::IntegrateExternalPotential(const std::vector<double>& Potentialvalues){

            ub::matrix<double> ExternalMat = ub::zero_matrix<double>(_basis->AOBasisSize(), _basis->AOBasisSize());
            unsigned nthreads = 1;
            #ifdef _OPENMP
               nthreads = omp_get_max_threads();
            #endif
               std::vector<ub::matrix<double> >vex_thread;
               std::vector<double> Exc_thread=std::vector<double>(nthreads,0.0);
               for(unsigned i=0;i<nthreads;++i){
                   ub::matrix<double> Vex_thread=ub::zero_matrix<double>(ExternalMat.size1());
                   vex_thread.push_back(Vex_thread);
               }

            #pragma omp parallel for
            for (unsigned thread=0;thread<nthreads;++thread){
            for (unsigned i = thread_start[thread]; i < thread_stop[thread]; ++i) {

                GridBox& box = _grid_boxes[i];

                ub::matrix<double> Vex_here=ub::zero_matrix<double>(box.Matrixsize());
                const std::vector<tools::vec>& points=box.getGridPoints();
                const std::vector<double>& weights=box.getGridWeights();

                ub::range one=ub::range(0,1);

                ub::matrix<double> _temp     = ub::zero_matrix<double>(1,box.Matrixsize());

                ub::matrix<double> ao=ub::matrix<double>(1,box.Matrixsize());

                //iterate over gridpoints
                for(unsigned p=0;p<box.size();p++){
                    ao=ub::zero_matrix<double>(1,box.Matrixsize());
                    const std::vector<ub::range>& aoranges=box.getAOranges();
                    const std::vector<const AOShell* > shells=box.getShells();
                    for(unsigned j=0;j<box.Shellsize();++j){
                        const AOShell* shell=shells[j];
                        ub::matrix_range< ub::matrix<double> > aoshell=ub::project(ao,one,aoranges[j]);
                        shell->EvalAOspace(aoshell,points[p]);
                    }

                    double weight=weights[p];
                    ub::matrix<double> _addEX = weight*Potentialvalues[box.getIndexoffirstgridpoint()+p]*ao ;

                    Vex_here+=ub::prod( ub::trans(_addEX), ao);
                }

                box.AddtoBigMatrix(vex_thread[thread],Vex_here);

            }
            }
            for(unsigned i=0;i<nthreads;++i){
                ExternalMat+=vex_thread[i];

               }

            ExternalMat += ub::trans(ExternalMat);
            return ExternalMat;

        }

        void NumericalIntegration::setXCfunctional(const string _functional){

            Vxc_Functionals map;
            std::vector<string> strs;
            boost::split(strs, _functional, boost::is_any_of(" "));
            xfunc_id = 0;

#ifdef LIBXC
            _use_votca = false;
            _use_separate = false;
            cfunc_id = 0;

            if (strs.size() == 1) {
                xfunc_id = map.getID(strs[0]);
                if (xfunc_id < 0) _use_votca = true;
            }

            else if (strs.size() == 2) {
                cfunc_id = map.getID(strs[0]);
                xfunc_id = map.getID(strs[1]);
                _use_separate = true;
            }
            else {
                cout<<"LIBXC "<<strs.size()<<endl;
                throw std::runtime_error("With LIBXC. Please specify one combined or an exchange and a correlation functionals");

            }

            if (!_use_votca){
            if (xc_func_init(&xfunc, xfunc_id, XC_UNPOLARIZED) != 0) {
                fprintf(stderr, "Functional '%d' not found\n", xfunc_id);
                exit(1);
            }

            xc_func_init(&xfunc, xfunc_id, XC_UNPOLARIZED);
            if (xfunc.info->kind!=2 && !_use_separate){
                throw std::runtime_error("Your functional misses either correlation or exchange, please specify another functional, separated by whitespace");
            }

            if (_use_separate) {
                if (xc_func_init(&cfunc, cfunc_id, XC_UNPOLARIZED) != 0) {
                    fprintf(stderr, "Functional '%d' not found\n", cfunc_id);
                    exit(1);
                }
                xc_func_init(&cfunc, cfunc_id, XC_UNPOLARIZED);
                xc_func_init(&xfunc, xfunc_id, XC_UNPOLARIZED);
                if ((xfunc.info->kind+cfunc.info->kind)!=1){
                    throw std::runtime_error("Your functionals are not one exchange and one correlation");
                }
            }
            }
#else
         if (strs.size() == 1) {
                xfunc_id = map.getID(strs[0]);
            }
         else {
                throw std::runtime_error("Running without LIBXC, Please specify one combined or an exchange and a correlation functionals");
         }
#endif
            if(_use_votca){
                cout<<"Warning: VOTCA_PBE does give correct Vxc but incorrect E_xc"<<endl;
            }
            setXC=true;
            return;
        }

        void NumericalIntegration::EvaluateXC(const double rho,const ub::matrix<double>& grad_rho,double& f_xc, double& df_drho, double& df_dsigma){

 #ifdef LIBXC
                    if (_use_votca) {
#endif
                        _xc.getXC(xfunc_id, rho, grad_rho(0,0), grad_rho(0,1), grad_rho(0,2), f_xc, df_drho, df_dsigma);
#ifdef LIBXC
                    }                        // evaluate via LIBXC, if compiled, otherwise, go via own implementation

                    else {

                        double sigma = ub::prod(grad_rho,ub::trans(grad_rho))(0,0);

                        double exc[1];
                        double vsigma[1]; // libxc
                        double vrho[1]; // libxc df/drho
                        switch (xfunc.info->family) {
                            case XC_FAMILY_LDA:
                                xc_lda_exc_vxc(&xfunc, 1, &rho, exc, vrho);
                                break;
                            case XC_FAMILY_GGA:
                            case XC_FAMILY_HYB_GGA:
                                xc_gga_exc_vxc(&xfunc, 1, &rho, &sigma, exc, vrho, vsigma);
                                break;
                        }
                        f_xc = exc[0];
                        df_drho = vrho[0];
                        df_dsigma = vsigma[0];
                        if (_use_separate) {
                            // via libxc correlation part only
                            switch (cfunc.info->family) {
                                case XC_FAMILY_LDA:
                                    xc_lda_exc_vxc(&cfunc, 1, &rho, exc, vrho);
                                    break;
                                case XC_FAMILY_GGA:
                                case XC_FAMILY_HYB_GGA:
                                    xc_gga_exc_vxc(&cfunc, 1, &rho, &sigma, exc, vrho, vsigma);
                                    break;
                            }

                            f_xc += exc[0];
                            df_drho += vrho[0];
                            df_dsigma += vsigma[0];
                        }
                    }
#endif

            return;
        }

        double NumericalIntegration::IntegratePotential(const vec& rvector){

            double result = 0.0;
            assert(density_set && "Density not calculated");

            for (unsigned i = 0; i < _grid_boxes.size(); i++) {

                const std::vector<tools::vec>& points = _grid_boxes[i].getGridPoints();
                const std::vector<double>& weights = _grid_boxes[i].getGridWeights();
                const std::vector<double>& densities = _grid_boxes[i].getGridDensities();
                for (unsigned j = 0; j < points.size(); j++) {
                    double dist = abs(points[j] - rvector);
                    result -= weights[j] * densities[j] / dist;
                }
            }

            return result;
        }

        void NumericalIntegration::SortGridpointsintoBlocks(std::vector< std::vector< GridContainers::integration_grid > >& grid){
            const double boxsize=1;

            std::vector< std::vector< std::vector< std::vector< GridContainers::integration_grid* > > > >  boxes;

            tools::vec min=vec(std::numeric_limits<double>::max());
            tools::vec max=vec(std::numeric_limits<double>::min());

            for ( unsigned i = 0 ; i < grid.size(); i++){
                for ( unsigned j = 0 ; j < grid[i].size(); j++){
                    const tools::vec& pos= grid[i][j].grid_pos;
                    if(pos.getX()>max.getX()){
                        max.x()=pos.getX();
                    }
                    else if(pos.getX()<min.getX()){
                        min.x()=pos.getX();
                    }
                    if(pos.getY()>max.getY()){
                        max.y()=pos.getY();
                    }
                    else if(pos.getY()<min.getY()){
                        min.y()=pos.getY();
                    }
                    if(pos.getZ()>max.getZ()){
                        max.z()=pos.getZ();
                    }
                    else if(pos.getZ()<min.getZ()){
                        min.z()=pos.getZ();
                        }
                    }
                }

            vec molextension=(max-min);
            vec numberofboxes=molextension/boxsize;
            vec roundednumofbox=vec(std::ceil(numberofboxes.getX()),std::ceil(numberofboxes.getY()),std::ceil(numberofboxes.getZ()));

            //creating temparray
            for (unsigned i=0;i<unsigned(roundednumofbox.getX());i++){
                std::vector< std::vector< std::vector< GridContainers::integration_grid* > > > boxes_yz;
                for (unsigned j=0;j<unsigned(roundednumofbox.getY());j++){
                    std::vector< std::vector< GridContainers::integration_grid* > >  boxes_z;
                    for (unsigned k=0;k<unsigned(roundednumofbox.getZ());k++){
                        std::vector< GridContainers::integration_grid* >  box;
                        box.reserve(100);
                        boxes_z.push_back(box);
                    }
                    boxes_yz.push_back(boxes_z);
            }
                boxes.push_back(boxes_yz);
            }

             for ( auto & atomgrid : grid){
                for ( auto & gridpoint : atomgrid){
                    tools::vec pos= gridpoint.grid_pos-min;
                    tools::vec index=pos/boxsize;
                    int i_x=int(index.getX());
                    int i_y=int(index.getY());
                    int i_z=int(index.getZ());
                    boxes[i_x][i_y][i_z].push_back(&gridpoint);
                }
             }

            for ( auto& boxes_xy : boxes){
                for( auto& boxes_z : boxes_xy){
                    for ( auto& box : boxes_z){
                        if( box.size()<1){
                            continue;
                        }
                        GridBox gridbox;

                        for(const auto&point:box){
                            gridbox.addGridPoint(*point);
                        }
                        _grid_boxes.push_back(gridbox);
                    }
                }
            }

            return;
        }

        void NumericalIntegration::FindSignificantShells(){

            for (unsigned i=0;i<_grid_boxes.size();++i){
                GridBox & box=_grid_boxes[i];
                for (AOBasis::AOShellIterator _row = _basis->firstShell(); _row != _basis->lastShell(); _row++) {
                      AOShell* _store=(*_row);
                      const double decay=(*_row)->getMinDecay();
                      const tools::vec& shellpos=(*_row)->getPos();

                      for(const auto& point : box.getGridPoints()){
                          tools::vec dist=shellpos-point;
                          double distsq=dist*dist;
                          // if contribution is smaller than -ln(1e-10), add atom to list
                        if ( (decay * distsq) < 20.7 ){
                            box.addShell(_store);
                            break;
                        }
                      }
                }
                //cout<<box.significant_shells.size()<<" "<<box.grid_pos.size()<<endl;
            }

             std::vector< GridBox > _grid_boxes_copy;

            int combined=0;
            std::vector<bool> Compared=std::vector<bool>(_grid_boxes.size(),false);
            for (unsigned i=0;i<_grid_boxes.size();i++){
                if(Compared[i]){continue;}
                GridBox box=_grid_boxes[i];
                if(box.Shellsize()<1){continue;}
                Compared[i]=true;
                for (unsigned j=i+1;j<_grid_boxes.size();j++){
                    if(GridBox::compareGridboxes(_grid_boxes[i],_grid_boxes[j])){
                        Compared[j]=true;
                        box.addGridBox(_grid_boxes[j]);
                        combined++;
                    }

                }
                _grid_boxes_copy.push_back(box);
            }

            std::vector<unsigned> sizes;
            sizes.reserve(_grid_boxes_copy.size());
            for(auto& box: _grid_boxes_copy){
                sizes.push_back(box.size()*box.Matrixsize());
            }

            std::vector<unsigned> indexes=std::vector<unsigned>(sizes.size());
            std::iota(indexes.begin(), indexes.end(), 0);
            std::sort(indexes.begin(), indexes.end(),[&sizes](unsigned i1, unsigned i2) {return sizes[i1] > sizes[i2];});

             unsigned nthreads = 1;
            #ifdef _OPENMP
               nthreads = omp_get_max_threads();
            #endif

            std::vector<unsigned> scores=std::vector<unsigned>(nthreads,0);
            std::vector< std::vector<unsigned> > indices;
            for (unsigned i=0;i<nthreads;++i){
                std::vector<unsigned> thread_box_indices;
                indices.push_back(thread_box_indices);
            }

            for(const auto index:indexes){
                unsigned thread=0;
                unsigned minimum= std::numeric_limits<unsigned>::max();
                for(unsigned i=0;i<scores.size();++i){
                    if(scores[i]<minimum){
                        minimum=scores[i];
                        thread=i;
                    }
                }
                indices[thread].push_back(index);
                scores[thread]+=sizes[index];
            }

            thread_start=std::vector<unsigned>(0);
            thread_stop=std::vector<unsigned>(0);
            unsigned start=0;
            unsigned stop=0;
            unsigned indexoffirstgridpoint=0;
             _grid_boxes.resize(0);
            for (const std::vector<unsigned>& thread_index:indices){
                thread_start.push_back(start);
                stop=start+thread_index.size();
                thread_stop.push_back(stop);
                start=stop;
                for(const unsigned index:thread_index){
                        GridBox newbox=_grid_boxes_copy[index];
                        newbox.setIndexoffirstgridpoint(indexoffirstgridpoint);
                        indexoffirstgridpoint+=newbox.size();
                        newbox.PrepareForIntegration();
                        _grid_boxes.push_back(newbox);
                }
            }
            return;
        }

        ub::matrix<double> NumericalIntegration::IntegrateVXC(const ub::matrix<double>& _density_matrix){
            ub::matrix<double> Vxc=ub::zero_matrix<double>(_density_matrix.size1());
            EXC = 0;

            unsigned nthreads = 1;
            #ifdef _OPENMP
               nthreads = omp_get_max_threads();
            #endif
               std::vector<ub::matrix<double> >vxc_thread;
               std::vector<double> Exc_thread=std::vector<double>(nthreads,0.0);
               for(unsigned i=0;i<nthreads;++i){
                   ub::matrix<double> Vxc_thread=ub::zero_matrix<double>(_density_matrix.size1());
                   vxc_thread.push_back(Vxc_thread);
               }

            #pragma omp parallel for
             for (unsigned thread=0;thread<nthreads;++thread){
                for (unsigned i = thread_start[thread]; i < thread_stop[thread]; ++i) {

                double EXC_box=0.0;
                GridBox& box = _grid_boxes[i];

                const ub::matrix<double>  DMAT_here=box.ReadFromBigMatrix(_density_matrix);

                ub::matrix<double> Vxc_here=ub::zero_matrix<double>(DMAT_here.size1());
                const std::vector<tools::vec>& points=box.getGridPoints();
                const std::vector<double>& weights=box.getGridWeights();

                ub::range one=ub::range(0,1);
                ub::range three=ub::range(0,3);
                ub::matrix<double> _temp     = ub::matrix<double>(1,box.Matrixsize());
                ub::matrix<double> _tempgrad = ub::matrix<double>(3,box.Matrixsize());
                ub::matrix<double> ao=ub::matrix<double>(1,box.Matrixsize());
                ub::matrix<double> ao_grad=ub::matrix<double>(3,box.Matrixsize());

                //iterate over gridpoints
                for(unsigned p=0;p<box.size();p++){
                    ao=ub::zero_matrix<double>(1,box.Matrixsize());
                    ao_grad=ub::zero_matrix<double>(3,box.Matrixsize());
                    const std::vector<ub::range>& aoranges=box.getAOranges();
                    const std::vector<const AOShell* >& shells=box.getShells();

                    for(unsigned j=0;j<box.Shellsize();++j){

                        ub::matrix_range< ub::matrix<double> > aoshell=ub::project(ao,one,aoranges[j]);
                        ub::matrix_range< ub::matrix<double> > ao_grad_shell=ub::project(ao_grad,three,aoranges[j]);
                        shells[j]->EvalAOspace(aoshell,ao_grad_shell,points[p]);

                    }

                    _temp=ub::prod( ao, DMAT_here);

                    _tempgrad=ub::prod(ao_grad,DMAT_here);

                    double rho=ub::prod(_temp, ub::trans( ao) )(0,0);

                    ub::matrix<double> rho_grad=ub::prod(_temp, ub::trans(ao_grad))+ub::prod(ao,ub::trans(_tempgrad));

		    if ( rho < 1.e-15 ) continue; // skip the rest, if density is very small

                    double f_xc;      // E_xc[n] = int{n(r)*eps_xc[n(r)] d3r} = int{ f_xc(r) d3r }
                    double df_drho;   // v_xc_rho(r) = df/drho
                    double df_dsigma; // df/dsigma ( df/dgrad(rho) = df/dsigma * dsigma/dgrad(rho) = df/dsigma * 2*grad(rho))
                    EvaluateXC( rho,rho_grad,f_xc, df_drho, df_dsigma);

                    double weight=weights[p];
                    ub::matrix<double> _addXC = weight * df_drho * ao *0.5;

                    _addXC+=  2.0*df_dsigma * weight * ub::prod(rho_grad,ao_grad);

                    // Exchange correlation energy
                    EXC_box += weight  * rho * f_xc;

                    Vxc_here+=ub::prod( ub::trans(_addXC), ao);

                }

                box.AddtoBigMatrix(vxc_thread[thread],Vxc_here);

                Exc_thread[thread]+=EXC_box;

            }

            }
            for(unsigned i=0;i<nthreads;++i){
                Vxc+=vxc_thread[i];
                EXC+=Exc_thread[i];
               }
            Vxc+=ub::trans(Vxc);

            return Vxc;
        }

        double NumericalIntegration::IntegrateDensity(const ub::matrix<double>& _density_matrix){

            double N = 0;

            unsigned nthreads = 1;
            #ifdef _OPENMP
               nthreads = omp_get_max_threads();
            #endif

               std::vector<double> N_thread=std::vector<double>(nthreads,0.0);

            #pragma omp parallel for
            for (unsigned thread=0;thread<nthreads;++thread){
             for (unsigned i = thread_start[thread]; i < thread_stop[thread]; ++i) {

                double N_box=0.0;
                GridBox& box = _grid_boxes[i];

                const ub::matrix<double>  DMAT_here=box.ReadFromBigMatrix(_density_matrix);

                ub::matrix<double> Vxc_here=ub::zero_matrix<double>(DMAT_here.size1());
                const std::vector<tools::vec>& points=box.getGridPoints();
                const std::vector<double>& weights=box.getGridWeights();

                ub::range one=ub::range(0,1);

                ub::matrix<double> _temp     = ub::zero_matrix<double>(1,box.Matrixsize());

                ub::matrix<double> ao=ub::matrix<double>(1,box.Matrixsize());

                box.prepareDensity();

                //iterate over gridpoints
                for(unsigned p=0;p<box.size();p++){
                    ao=ub::zero_matrix<double>(1,box.Matrixsize());

                    const std::vector<ub::range>& aoranges=box.getAOranges();
                    const std::vector<const AOShell* > shells=box.getShells();
                    for(unsigned j=0;j<box.Shellsize();++j){
                        const AOShell* shell=shells[j];
                        ub::matrix_range< ub::matrix<double> > aoshell=ub::project(ao,one,aoranges[j]);

                        shell->EvalAOspace(aoshell,points[p]);
                    }

                    _temp=ub::prod( ao, DMAT_here);

                    double rho=ub::prod(_temp, ub::trans( ao) )(0,0);
                    box.addDensity(rho);
                    N_box+=rho*weights[p];

                }

                N_thread[thread]+=N_box;

            }
            }
            for(unsigned i=0;i<nthreads;++i){
                N+=N_thread[i];
               }
            density_set=true;
            return N;
        }

 ub::vector<double> NumericalIntegration::IntegrateGyrationTensor(const ub::matrix<double>& _density_matrix){

            double N = 0;
            double centroid_x = 0.0;
            double centroid_y = 0.0;
            double centroid_z = 0.0;
            double gyration_xx = 0.0;
            double gyration_xy = 0.0;
            double gyration_xz = 0.0;
            double gyration_yy = 0.0;
            double gyration_yz = 0.0;
            double gyration_zz = 0.0;
            ub::vector<double> result=ub::zero_vector<double>(10);

            unsigned nthreads = 1;
            #ifdef _OPENMP
               nthreads = omp_get_max_threads();
            #endif

               std::vector<double> N_thread=std::vector<double>(nthreads,0.0);

               // centroid
	       std::vector<double> centroid_x_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> centroid_y_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> centroid_z_thread=std::vector<double>(nthreads,0.0);

	       // gyration tensor
	       std::vector<double> gyration_xx_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> gyration_xy_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> gyration_xz_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> gyration_yy_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> gyration_yz_thread=std::vector<double>(nthreads,0.0);
	       std::vector<double> gyration_zz_thread=std::vector<double>(nthreads,0.0);

            #pragma omp parallel for
            for (unsigned thread=0;thread<nthreads;++thread){
             for (unsigned i = thread_start[thread]; i < thread_stop[thread]; ++i) {

                double N_box=0.0;
                double centroid_x_box=0.0;
                double centroid_y_box=0.0;
                double centroid_z_box=0.0;
                double gyration_xx_box=0.0;
                double gyration_xy_box=0.0;
                double gyration_xz_box=0.0;
                double gyration_yy_box=0.0;
                double gyration_yz_box=0.0;
                double gyration_zz_box=0.0;

                GridBox& box = _grid_boxes[i];

                const ub::matrix<double>  DMAT_here=box.ReadFromBigMatrix(_density_matrix);

                const std::vector<tools::vec>& points=box.getGridPoints();
                const std::vector<double>& weights=box.getGridWeights();

                ub::range one=ub::range(0,1);

                ub::matrix<double> _temp     = ub::zero_matrix<double>(1,box.Matrixsize());

                ub::matrix<double> ao=ub::matrix<double>(1,box.Matrixsize());

                box.prepareDensity();

                //iterate over gridpoints
                for(unsigned p=0;p<box.size();p++){
                    ao=ub::zero_matrix<double>(1,box.Matrixsize());

                    const std::vector<ub::range>& aoranges=box.getAOranges();
                    const std::vector<const AOShell* > shells=box.getShells();
                    for(unsigned j=0;j<box.Shellsize();++j){
                        const AOShell* shell=shells[j];
                        ub::matrix_range< ub::matrix<double> > aoshell=ub::project(ao,one,aoranges[j]);

                        shell->EvalAOspace(aoshell,points[p]);
                    }

                    _temp=ub::prod( ao, DMAT_here);

                    double rho=ub::prod(_temp, ub::trans( ao) )(0,0);
                    box.addDensity(rho);
                    N_box+=rho*weights[p];
                    centroid_x_box+=rho*weights[p]* points[p].getX();
                    centroid_y_box+=rho*weights[p]* points[p].getY();
                    centroid_z_box+=rho*weights[p]* points[p].getZ();
                    gyration_xx_box+=rho*weights[p]* points[p].getX()*points[p].getX();
                    gyration_xy_box+=rho*weights[p]* points[p].getX()*points[p].getY();
                    gyration_xz_box+=rho*weights[p]* points[p].getX()*points[p].getZ();
                    gyration_yy_box+=rho*weights[p]* points[p].getY()*points[p].getY();
                    gyration_yz_box+=rho*weights[p]* points[p].getY()*points[p].getZ();
                    gyration_zz_box+=rho*weights[p]* points[p].getZ()*points[p].getZ();

                }

                N_thread[thread]+=N_box;
                centroid_x_thread[thread] += centroid_x_box;
                centroid_y_thread[thread] += centroid_y_box;
                centroid_z_thread[thread] += centroid_z_box;
                gyration_xx_thread[thread] += gyration_xx_box;
                gyration_xy_thread[thread] += gyration_xy_box;
                gyration_xz_thread[thread] += gyration_xz_box;
                gyration_yy_thread[thread] += gyration_yy_box;
                gyration_yz_thread[thread] += gyration_yz_box;
                gyration_zz_thread[thread] += gyration_zz_box;

            }
            }
            for(unsigned i=0;i<nthreads;++i){
                N+=N_thread[i];
                centroid_x += centroid_x_thread[i];
                centroid_y += centroid_y_thread[i];
                centroid_z += centroid_z_thread[i];
                gyration_xx += gyration_xx_thread[i];
                gyration_xy += gyration_xy_thread[i];
                gyration_xz += gyration_xz_thread[i];
                gyration_yy += gyration_yy_thread[i];
                gyration_yz += gyration_yz_thread[i];
                gyration_zz += gyration_zz_thread[i];
               }
            density_set=true;

            // Normalize
	    centroid_x = centroid_x/N;
	    centroid_y = centroid_y/N;
	    centroid_z = centroid_z/N;

            gyration_xx = gyration_xx/N;
	    gyration_xy = gyration_xy/N;
	    gyration_xz = gyration_xz/N;
	    gyration_yy = gyration_yy/N;
	    gyration_yz = gyration_yz/N;
	    gyration_zz = gyration_zz/N;

	      // Copy all elements to result vector
	      result(0) = N;
	      result(1) = centroid_x;
	      result(2) = centroid_y;
	      result(3) = centroid_z;
	      result(4) = gyration_xx - centroid_x*centroid_x;
	      result(5) = gyration_xy - centroid_x*centroid_y;
	      result(6) = gyration_xz - centroid_x*centroid_z;
	      result(7) = gyration_yy - centroid_y*centroid_y;
	      result(8) = gyration_yz - centroid_y*centroid_z;
	      result(9) = gyration_zz - centroid_z*centroid_z;

            return result;
        }

        std::vector<const vec *> NumericalIntegration::getGridpoints(){

            std::vector<const vec *> gridpoints;

            for ( unsigned i = 0 ; i < _grid_boxes.size(); i++){
                const std::vector<tools::vec>& points=_grid_boxes[i].getGridPoints();
                for ( unsigned j = 0 ; j < points.size(); j++){
                    gridpoints.push_back(&points[j]);

               }
                }
            return gridpoints;
        }

        void NumericalIntegration::GridSetup(string type, BasisSet* bs, vector<ctp::QMAtom*> _atoms,AOBasis* basis) {
            _basis=basis;
            std::vector< std::vector< GridContainers::integration_grid > > grid;
            const double pi = boost::math::constants::pi<double>();
            // get GridContainer
            GridContainers initialgrids;

            // get radial grid per element
            EulerMaclaurinGrid _radialgrid;
            _radialgrid.getRadialGrid(bs, _atoms, type, initialgrids); // this checks out 1:1 with NWChem results! AWESOME

           map<string, GridContainers::radial_grid>::iterator it;

            LebedevGrid _sphericalgrid;

            for (it = initialgrids._radial_grids.begin(); it != initialgrids._radial_grids.end(); ++it) {
               _sphericalgrid.getSphericalGrid(_atoms, type, initialgrids);

            }

            // for the partitioning, we need all inter-center distances later, stored in one-directional list
            int ij = 0;
            Rij.push_back(0.0); // 1st center "self-distance"

            vector< ctp::QMAtom* > ::iterator ait;
            vector< ctp::QMAtom* > ::iterator bit;
            int i = 1;
            for (ait = _atoms.begin() + 1; ait != _atoms.end(); ++ait) {
                // get center coordinates in Bohr
                vec pos_a = (*ait)->getPos() * tools::conv::ang2bohr;

                int j = 0;
                for (bit = _atoms.begin(); bit != ait; ++bit) {
                    ij++;
                    // get center coordinates in Bohr
                    vec pos_b = (*bit)->getPos() * tools::conv::ang2bohr;

                    Rij.push_back(1.0 / abs(pos_a-pos_b));

                    j++;
                } // atoms
                Rij.push_back(0.0); // self-distance again
                i++;
            } // atoms

            int i_atom = 0;
            _totalgridsize = 0;
            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {
                // get center coordinates in Bohr
                std::vector< GridContainers::integration_grid > _atomgrid;
                const vec atomA_pos =(*ait)->getPos() * tools::conv::ang2bohr;

                string name = (*ait)->type;

                // get radial grid information for this atom type
                GridContainers::radial_grid _radial_grid = initialgrids._radial_grids.at(name);

                // get spherical grid information for this atom type
                GridContainers::spherical_grid _spherical_grid = initialgrids._spherical_grids.at(name);

                // maximum order (= number of points) in spherical integration grid
                int maxorder = _sphericalgrid.Type2MaxOrder(name,type);
                int maxindex = _sphericalgrid.getIndexFromOrder(maxorder);

                // for pruning of integration grid, get interval boundaries for this element
                std::vector<double> PruningIntervals = _radialgrid.getPruningIntervals( name );
              //  cout << " Pruning Intervals: " << PruningIntervals[0] << " " << PruningIntervals[1] << " " << PruningIntervals[2] << " " << PruningIntervals[3] << endl;

                int current_order = 0;
                // get spherical grid
                std::vector<double> _theta;
                std::vector<double> _phi;
                std::vector<double> _weight;

                // for each radial value
                for (unsigned _i_rad = 0; _i_rad < _radial_grid.radius.size(); _i_rad++) {
                    double r = _radial_grid.radius[_i_rad];
                    int order;
                    // which Lebedev order for this point?
                    if ( maxindex == 1 ) {
                        // smallest possible grid anyway, nothing to do
                        order = maxorder;
                    } else if ( maxindex == 2 ) {
                        // only three intervals
                        if ( r < PruningIntervals[0] ) {
                            order = _sphericalgrid.getOrderFromIndex(1);//1;
                        } else if ( ( r >= PruningIntervals[0] ) && ( r < PruningIntervals[3] )   ){
                            order = _sphericalgrid.getOrderFromIndex(2);
                        } else {
                            order = _sphericalgrid.getOrderFromIndex(1);
                        } // maxorder == 2
                    } else {
                        // five intervals
                        if ( r < PruningIntervals[0] ) {
                            order = _sphericalgrid.getOrderFromIndex(int(2));
                        } else if ( ( r >= PruningIntervals[0]) && ( r < PruningIntervals[1] ) ) {
                            order = _sphericalgrid.getOrderFromIndex(4);
                        } else if ( ( r >= PruningIntervals[1]) && ( r < PruningIntervals[2] ) ) {
                            order = _sphericalgrid.getOrderFromIndex(max(maxindex-1, 4));
                        } else if ( (r >= PruningIntervals[2]) && ( r < PruningIntervals[3] ) ) {
                            order = maxorder;
                        } else {
                            order = _sphericalgrid.getOrderFromIndex(max(maxindex-1,1));
                        }
                    }

                    // get new spherical grid, if order changed
                    if ( order != current_order ){
                        _theta.clear();
                        _phi.clear();
                        _weight.clear();

                        _sphericalgrid.getUnitSphereGrid(order,_theta,_phi,_weight);
                        current_order = order;
                    }

                    for (unsigned _i_sph = 0; _i_sph < _phi.size(); _i_sph++) {

                        double p   = _phi[_i_sph] * pi / 180.0; // back to rad
                        double t   = _theta[_i_sph] * pi / 180.0; // back to rad
                        double ws  = _weight[_i_sph];

                        const vec s = vec(sin(p) * cos(t), sin(p) * sin(t),cos(p));

                        GridContainers::integration_grid _gridpoint;
                        _gridpoint.grid_pos = atomA_pos+r*s;

                        _gridpoint.grid_weight = _radial_grid.weight[_i_rad] * ws;

                        _atomgrid.push_back(_gridpoint);

                    } // spherical gridpoints
                } // radial gridpoint

                // get all distances from grid points to centers
                std::vector< std::vector<double> > rq;
                // for each center
                for (bit = _atoms.begin(); bit < _atoms.end(); ++bit) {
                    // get center coordinates
                   const vec atom_pos = (*bit)->getPos() * tools::conv::ang2bohr;

                    std::vector<double> temp;
                    // for each gridpoint
                    for (std::vector<GridContainers::integration_grid >::iterator git = _atomgrid.begin(); git != _atomgrid.end(); ++git) {

                        temp.push_back(abs(git->grid_pos-atom_pos));

                    } // gridpoint of _atomgrid
                    rq.push_back(temp);

                } // centers
                // cout << " Calculated all gridpoint distances to centers for " << i_atom << endl;

                // find nearest-neighbor of this atom
                double distNN = 1e10;

                vector< ctp::QMAtom* > ::iterator NNit;
                //int i_NN;

                // now check all other centers
                int i_b =0;
                for (bit = _atoms.begin(); bit != _atoms.end(); ++bit) {

                    if (bit != ait) {
                        // get center coordinates

                        const vec atomB_pos=(*bit)->getPos() * tools::conv::ang2bohr;
                        double distSQ = (atomA_pos-atomB_pos)*(atomA_pos-atomB_pos);

                        // update NN distance and iterator
                        if ( distSQ < distNN ) {
                            distNN = distSQ;
                            NNit = bit;

                        }

                    } // if ( ait != bit)
                    i_b++;
                }// bit centers

                for ( unsigned i_grid = 0; i_grid < _atomgrid.size() ; i_grid++){
                    // call some shit called grid_ssw0 in NWChem
                    std::vector<double> _p = SSWpartition( i_grid, _atoms.size(),rq);

                    // check weight sum
                    double wsum = 0.0;
                    for (unsigned i =0 ; i < _p.size(); i++ ){
                        wsum += _p[i];
                    }
                    //cout << " sum of partition weights " << wsum << endl;
                    if ( wsum != 0.0 ){

                        // update the weight of this grid point
                        _atomgrid[i_grid].grid_weight = _atomgrid[i_grid].grid_weight * _p[i_atom]/wsum;
                        //cout << " adjusting gridpoint weight "  << endl;
                    } else {

                       cerr << "\nSum of partition weights of grid point " << i_grid << " of atom " << i_atom << " is zero! ";
                       throw std::runtime_error("\nThis should never happen!");

                    }

                } // partition weight for each gridpoint

                // now remove points from the grid with negligible weights

                for (std::vector<GridContainers::integration_grid >::iterator git = _atomgrid.begin(); git != _atomgrid.end();) {
                    if (git->grid_weight < 1e-13 ) {
                        git = _atomgrid.erase(git);
                    } else {
                        ++git;
                    }
                }

                _totalgridsize += _atomgrid.size() ;

                grid.push_back(_atomgrid);

                i_atom++;

            } // atoms

            SortGridpointsintoBlocks(grid);
            FindSignificantShells();
            return;
        }

        std::vector<double> NumericalIntegration::SSWpartition(int igrid, int ncenters, std::vector< std::vector<double> >& rq){
            const double ass = 0.725;
            // initialize partition vector to 1.0
            std::vector<double> p(ncenters,1.0);

            const double tol_scr = 1e-10;
            const double leps    = 1e-6;
            // go through centers
            for ( int i = 1; i < ncenters; i++ ){

                int ij = i*(i+1)/2 -1; // indexing magic
                double rag = rq[i][igrid] ;

                // through all other centers (one-directional)
                for (int j = 0; j < i; j++ ){

                    ij++;
                    if ( ( std::abs(p[i]) > tol_scr  ) || ( std::abs(p[j]) > tol_scr  ) ){

                        double mu = ( rag - rq[j][igrid] )*Rij[ij];
                        if ( mu > ass ) {
                            p[i] = 0.0;
                        } else if ( mu < -ass ) {
                            p[j] = 0.0;
                        } else {

                            double sk;
                            if (std::abs(mu) < leps ) {
                                sk = -1.88603178008*mu + 0.5;
                            } else {
                                sk = erf1c(mu);
                            }
                            if ( mu > 0.0 ) sk = 1.0 - sk;
                            p[j] = p[j] * sk;
                            p[i] = p[i] * (1.0-sk);

                        }
                    }
                }

            }

            return p;
        }

        double NumericalIntegration::erf1c(double x){

            const static double alpha_erf1=1.0/0.30;
            return 0.5*erfcc((x/(1.0-x*x))*alpha_erf1);
        }

        double NumericalIntegration::erfcc(double x){

            double tau = 1.0/(1.0+0.5*std::abs(x));

            return tau*exp(-x*x-1.26551223 + 1.00002368*tau + 0.37409196*tau*tau
            + 0.09678418*pow(tau,3) - 0.18628806*pow(tau,4) + 0.27886807*pow(tau,5)
            -1.13520398*pow(tau,6) + 1.48851587*pow(tau,7)  -0.82215223*pow(tau,8)
            + 0.17087277*pow(tau,9));
        }

    }
}


OBSOLETE

// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Use, modification and distribution is subject to 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)
//
// Doxygen Examples, referred to from the sources

#include <boost/tuple/tuple.hpp>

#if defined(_MSC_VER)
// We deliberately mix float/double's here so turn off warning
#pragma warning( disable : 4244 )
#endif // defined(_MSC_VER)

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

#include <boost/geometry/io/wkt/wkt.hpp>

// All functions below are referred to in the documentation of Boost.Geometry
// Don't rename them.
void example_area_polygon()
{
    //[area_polygon
    //` Calculate the area of a polygon
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly; /*< Declare >*/
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly); /*< Fill, in this case with WKT >*/
    double area = boost::geometry::area(poly); /*< Calculate area >*/
    //]

    //[area_polygon_spherical
    //` Calculate the area of a *spherical* polygon
    namespace bg = boost::geometry;
    bg::polygon<bg::point<float, 2, bg::cs::spherical<bg::degree> > > sph_poly;
    bg::read_wkt("POLYGON((0 0,0 45,45 0,0 0))", sph_poly);
    double area = bg::area(sph_poly);
    //]
}

void example_as_wkt_point()
{
    typedef boost::geometry::point_xy<double> P;
    P p(5.12, 6.34);
    // Points can be streamed like this:
    std::cout << boost::geometry::dsv<P>(p) << std::endl;

    // or like this:
    std::cout << boost::geometry::dsv(p) << std::endl;

    // or (with extension) like this:
    std::cout << boost::geometry::wkt(p) << std::endl;
}

void example_as_wkt_vector()
{
    std::vector<boost::geometry::point_xy<int> > v;
    boost::geometry::read_wkt<boost::geometry::point_xy<int> >("linestring(1 1,2 2,3 3,4 4)", std::back_inserter(v));

    std::cout << boost::geometry::dsv(std::make_pair(v.begin(), v.end())) << std::endl;
}

void example_centroid_polygon()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly);
    // Center of polygon might have different type than points of polygon
    boost::geometry::point_xy<float> center;
    boost::geometry::centroid(poly, center);
    std::cout << "Centroid: " << boost::geometry::dsv(center) << std::endl;
}

void example_distance_point_point()
{
    boost::geometry::point_xy<double> p1(1, 1);
    boost::geometry::point_xy<double> p2(2, 3);
    std::cout << "Distance p1-p2 is "
        << boost::geometry::distance(p1, p2)
        << " units" << std::endl;

    /*
    Extension, other coordinate system:
    // Read 2 Dutch cities from WKT texts (in decimal degrees)
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> >  a, r;
    boost::geometry::read_wkt("POINT(4.89222 52.3731)", a);
    boost::geometry::read_wkt("POINT(4.47917 51.9308)", r);

    std::cout << "Distance Amsterdam-Rotterdam is "
        << boost::geometry::distance(a, r) / 1000.0
        << " kilometers " << std::endl;
    */
}

void example_distance_point_point_strategy()
{
    /*
    Extension, other coordinate system:
    typedef boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    LL a, r;
    boost::geometry::read_wkt("POINT(4.89222 52.3731)", a);
    boost::geometry::read_wkt("POINT(4.47917 51.9308)", r);

    std::cout << "Distance Amsterdam-Rotterdam is "
        << boost::geometry::distance(a, r,
                boost::geometry::strategy::distance::vincenty<LL>() )
                 / 1000.0
        << " kilometers " << std::endl;
    */
}

void example_from_wkt_point()
{
    boost::geometry::point_xy<int> point;
    boost::geometry::read_wkt("Point(1 2)", point);
    std::cout << point.x() << "," << point.y() << std::endl;
}

void example_from_wkt_output_iterator()
{
    std::vector<boost::geometry::point_xy<int> > v;
    boost::geometry::read_wkt<boost::geometry::point_xy<int> >("linestring(1 1,2 2,3 3,4 4)", std::back_inserter(v));
    std::cout << "vector has " << v.size() << " coordinates" << std::endl;
}

void example_from_wkt_linestring()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(1 1,2 2,3 3,4 4)", line);
    std::cout << "linestring has " << line.size() << " coordinates" << std::endl;
}

void example_from_wkt_polygon()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 1,1 1,1 0,0 0))", poly);
    std::cout << "Polygon has " << poly.outer().size() << " coordinates in outer ring" << std::endl;
}

void example_point_ll_convert()
{
    /*
    Extension, other coordinate system:
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > deg(boost::geometry::latitude<>(33.0), boost::geometry::longitude<>(-118.0));
    boost::geometry::point_ll<double, boost::geometry::cs::geographic<boost::geometry::radian> > rad;
    boost::geometry::transform(deg, rad);

    std::cout << "point in radians: " << rad << std::endl;
    */
}

void example_clip_linestring1()
{
    typedef boost::geometry::point_xy<double> P;
    boost::geometry::linestring<P> line;
    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);
    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    std::cout << "Clipped linestring(s) " << std::endl;

    std::vector<boost::geometry::linestring<P> > intersection;
    boost::geometry::intersection_inserter<boost::geometry::linestring<P> >(cb, line, std::back_inserter(intersection));
}

void example_clip_linestring2()
{
    typedef boost::geometry::point_xy<double> P;
    std::vector<P> vector_in;
    boost::geometry::read_wkt<P>("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)",
                    std::back_inserter(vector_in));

    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    typedef std::vector<std::vector<P> > VV;
    VV vector_out;
    boost::geometry::intersection_inserter<std::vector<P>  >(cb, vector_in, std::back_inserter(vector_out));

    std::cout << "Clipped vector(s) " << std::endl;
    for (VV::const_iterator it = vector_out.begin(); it != vector_out.end(); it++)
    {
        std::copy(it->begin(), it->end(), std::ostream_iterator<P>(std::cout, " "));
        std::cout << std::endl;
    }
}

void example_intersection_polygon1()
{
    typedef boost::geometry::point_xy<double> P;
    typedef std::vector<boost::geometry::polygon<P> > PV;

    boost::geometry::box<P> cb(P(1.5, 1.5), P(4.5, 2.5));
    boost::geometry::polygon<P> poly;
    boost::geometry::read_wkt("POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            ",(4 2,4.2 1.4,4.8 1.9,4.4 2.2,4 2))", poly);

    PV v;
    boost::geometry::intersection_inserter<boost::geometry::polygon<P> >(cb, poly, std::back_inserter(v));

    std::cout << "Clipped polygon(s) " << std::endl;
    for (PV::const_iterator it = v.begin(); it != v.end(); it++)
    {
        std::cout << boost::geometry::dsv(*it) << std::endl;
    }
}

void example_simplify_linestring1()
{
    //[simplify
    //` Simplify a linestring
    boost::geometry::linestring<boost::geometry::point_xy<double> > line, simplified;
    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);
    boost::geometry::simplify(line, simplified, 0.5); /*< Simplify it, using distance of 0.5 units >*/
    std::cout
        << "  original line: " << boost::geometry::dsv(line) << std::endl
        << "simplified line: " << boost::geometry::dsv(simplified) << std::endl;
    //]
}

void example_simplify_linestring2()
{
    //[simplify_inserter
    //` Simplify a linestring using an output iterator
    typedef boost::geometry::point_xy<double> P;
    typedef boost::geometry::linestring<P> L;
    L line;

    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);

    typedef boost::geometry::strategy::distance::projected_point<P, P> DS;
    typedef boost::geometry::strategy::simplify::douglas_peucker<P, DS> simplification;
    boost::geometry::simplify_inserter(line, std::ostream_iterator<P>(std::cout, "\n"), 0.5, simplification());
    //]
}

void example_within()
{
    boost::geometry::polygon<boost::geometry::point_xy<double> > poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly);
    boost::geometry::point_xy<float> point(3, 3);
    std::cout << "Point is "
        << (boost::geometry::within(point, poly) ? "IN" : "NOT in")
        << " polygon"
        << std::endl;
}

/*
void example_within_strategy()
{
    // TO BE UPDATED/FINISHED
    typedef boost::geometry::point_xy<double> P;
    typedef boost::geometry::polygon<P> POLY;
    P p;
    std::cout << within(p, poly, strategy::within::cross_count<P>) << std::endl;
}
*/

void example_length_linestring()
{
    using namespace boost::geometry;
    linestring<point_xy<double> > line;
    read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    std::cout << "linestring length is "
        << length(line)
        << " units" << std::endl;

    /*
    Extension, other coordinate system:
    // Linestring in latlong, filled with
    // explicit degree-minute-second values
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    linestring<LL> line_ll;
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(52, 22, 23)),
        longitude<float>(dms<east, float>(4, 53, 32))));
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(51, 55, 51)),
        longitude<float>(dms<east, float>(4, 28, 45))));
    line_ll.push_back(LL(
        latitude<float>(dms<north, float>(52, 4, 48)),
        longitude<float>(dms<east, float>(4, 18, 0))));
    std::cout << "linestring length is "
        << length(line_ll) / 1000
        << " kilometers " << std::endl;
        */
}

void example_length_linestring_iterators1()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    std::cout << "linestring length is "
        << boost::geometry::length(line)
        << " units" << std::endl;
}

void example_length_linestring_iterators2()
{
    std::vector<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt<boost::geometry::point_xy<double> >("linestring(0 0,1 1,4 8,3 2)", std::back_inserter(line));
    std::cout << "linestring length is "
        << boost::geometry::length(line)
        << " units" << std::endl;
}

void example_length_linestring_iterators3()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    std::deque<LL> line;
    boost::geometry::read_wkt<LL>("linestring(0 51,1 51,2 52)", std::back_inserter(line));
    std::cout << "linestring length is "
        << 0.001 * boost::geometry::length(line, boost::geometry::strategy::distance::vincenty<LL>())
        << " kilometers" << std::endl;
    */
}

void example_length_linestring_strategy()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<float, boost::geometry::cs::geographic<boost::geometry::degree> > LL;
    linestring<LL> line_ll;
    line_ll.push_back(LL(latitude<float>(dms<north, float>(52, 22, 23)), longitude<float>(dms<east, float>(4, 53, 32))));
    line_ll.push_back(LL(latitude<float>(dms<north, float>(51, 55, 51)), longitude<float>(dms<east, float>(4, 28, 45))));
    line_ll.push_back(LL(latitude<float>(dms<north, float>(52, 4, 48)), longitude<float>(dms<east, float>(4, 18, 0))));
    std::cout << "linestring length is "
        << length(line_ll, strategy::distance::vincenty<LL, LL>() )/(1000)
        << " kilometers " << std::endl;
    */
}

void example_envelope_linestring()
{
    boost::geometry::linestring<boost::geometry::point_xy<double> > line;
    boost::geometry::read_wkt("linestring(0 0,1 1,4 8,3 2)", line);
    boost::geometry::box<boost::geometry::point_xy<double> > box;
    boost::geometry::envelope(line, box);

    std::cout << "envelope is " << boost::geometry::dsv(box) << std::endl;
}

void example_envelope_polygon()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> >  LL;

    // Wrangel island, 180 meridian crossing island above Siberia.
    polygon<LL> wrangel;
    wrangel.outer().push_back(LL(latitude<>(dms<north>(70, 47, 7)), longitude<>(dms<west>(178, 47, 9))));
    wrangel.outer().push_back(LL(latitude<>(dms<north>(71, 14, 0)), longitude<>(dms<east>(177, 28, 33))));
    wrangel.outer().push_back(LL(latitude<>(dms<north>(71, 34, 24)), longitude<>(dms<east>(179, 44, 37))));
    // Close it
    wrangel.outer().push_back(wrangel.outer().front());

    boost::geometry::box<LL> box;
    boost::geometry::envelope(wrangel, box);

    dms<cd_lat> minlat(box.min_corner().lat());
    dms<cd_lon> minlon(box.min_corner().lon());

    dms<cd_lat> maxlat(box.max_corner().lat());
    dms<cd_lon> maxlon(box.max_corner().lon());

    std::cout << wrangel << std::endl;
    std::cout << "min: " << minlat.get_dms() << " , " << minlon.get_dms() << std::endl;
    std::cout << "max: " << maxlat.get_dms() << " , " << maxlon.get_dms() << std::endl;
    */
}

void example_dms()
{
    /*
    Extension, other coordinate system:
    // Construction with degree/minute/seconds
    boost::geometry::dms<boost::geometry::east> d1(4, 53, 32.5);

    // Explicit conversion to double.
    std::cout << d1.as_value() << std::endl;

    // Conversion to string, with optional strings
    std::cout << d1.get_dms(" deg ", " min ", " sec") << std::endl;

    // Combination with latitude/longitude and cardinal directions
    {
        using namespace boost::geometry;
        point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > canberra(
            latitude<>(dms<south>(35, 18, 27)),
            longitude<>(dms<east>(149, 7, 27.9)));
        std::cout << canberra << std::endl;
    }
    */
}

void example_point_ll_construct()
{
    /*
    Extension, other coordinate system:
    using namespace boost::geometry;
    typedef point_ll<double, boost::geometry::cs::geographic<boost::geometry::degree> > ll;

    // Constructions in both orders possible
    ll juneau(
        latitude<>(dms<north>(58, 21, 5)),
        longitude<>(dms<west>(134, 30, 42)));
    ll wladiwostok(
        longitude<>(dms<east>(131, 54)),
        latitude<>(dms<north>(43, 8))
        );
    */
}

int main(void)
{
    example_area_polygon();

    example_centroid_polygon();

    example_distance_point_point();
    example_distance_point_point_strategy();

    example_from_wkt_point();
    example_from_wkt_output_iterator();
    example_from_wkt_linestring();
    example_from_wkt_polygon();

    example_as_wkt_point();

    example_clip_linestring1();
    example_clip_linestring2();
    example_intersection_polygon1();

    example_simplify_linestring1();
    example_simplify_linestring2();

    example_length_linestring();
    example_length_linestring_iterators1();
    example_length_linestring_iterators2();
    example_length_linestring_iterators3();
    example_length_linestring_strategy();

    example_envelope_linestring();
    example_envelope_polygon();

    example_within();

    example_point_ll_convert();
    example_point_ll_construct();
    example_dms();

    return 0;
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include <map>
#include <iostream>
#include <chrono>

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

#include "teaser_mex_utils.h"
#include "teaser/registration.h"

enum class INPUT_PARAMS : int {
  src = 0,
  dst = 1,
  cbar2 = 2,
  noise_bound = 3,
  estimate_scaling = 4,
  rotation_estimation_algorithm = 5,
  rotation_gnc_factor = 6,
  rotation_max_iterations = 7,
  rotation_cost_threshold = 8,
};

enum class OUTPUT_PARAMS : int {
  s_est = 0,
  R_est = 1,
  t_est = 2,
  time_taken = 3,
};

typedef bool (*mexTypeCheckFunction)(const mxArray*);
const std::map<INPUT_PARAMS, mexTypeCheckFunction> INPUT_PARMS_MAP{
    {INPUT_PARAMS::src, &isPointCloudMatrix},
    {INPUT_PARAMS::dst, &isPointCloudMatrix},
    {INPUT_PARAMS::cbar2, &isRealDoubleScalar},
    {INPUT_PARAMS::noise_bound, &isRealDoubleScalar},
    {INPUT_PARAMS::estimate_scaling, &mxIsLogicalScalar},
    {INPUT_PARAMS::rotation_estimation_algorithm, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_gnc_factor, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_max_iterations, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_cost_threshold, &isRealDoubleScalar},
};
const std::map<OUTPUT_PARAMS, mexTypeCheckFunction> OUTPUT_PARMS_MAP{
    {OUTPUT_PARAMS::s_est, &isRealDoubleScalar},
    {OUTPUT_PARAMS::R_est, &isRealDoubleMatrix<3, 3>},
    {OUTPUT_PARAMS::t_est, &isRealDoubleMatrix<3, 1>},
    {OUTPUT_PARAMS::time_taken, &isRealDoubleScalar},
};

/**
 * This is the MATLAB binding for TEASER++.
 *
 * Input:
 * - src: a 3-by-N matrix of 3D points representing points to be transformed
 * - dst: a 3-by-N matrix of 3D points representing points after transformation
 * - cbar2: square of maximum allowed ratio between noise and noise bound (see [1]).
 * - noise_bound: a floating-point number indicating the bound on noise
 * - estimate_scaling: a boolean indicating whether scale needs to be estimated
 * - rotation_max_iterations: maximum iterations for the rotation estimation loop
 * - rotation_cost_threshold: cost threshold for rotation termination
 * - rotation_gnc_factor: gnc factor for rotation estimation
 *                        for GNC-TLS method: it's multiplied on the GNC control parameter
 *                        for FGR method: it's divided on the GNC control parameter
 * - rotation_estimation_algorithm: a number indicating the rotation estimation method used;
 *                                  if it's 0: GNC-TLS
 *                                  if it's 1: FGR
 *
 * Output:
 * - s_est estimated scale (scalar)
 * - R_est estimated rotation matrix (3-by-3 matrix)
 * - t_est estimated translation vector (3-by-1 matrix)
 * - time_taken time it takes for the underlying TEASER++ library to compute a solution.
 *
 * [1] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,”
 * arXiv:2001.07715 [cs, math], Jan. 2020.
 *
 */
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {

  // Check for proper number of arguments
  if (nrhs != INPUT_PARMS_MAP.size()) {
    mexErrMsgIdAndTxt("teaserSolve:nargin", "Wrong number of input arguments.");
  }
  if (nlhs != OUTPUT_PARMS_MAP.size()) {
    mexErrMsgIdAndTxt("teaserSolve:nargin", "Wrong number of output arguments.");
  }

  // Check for proper input types
  for (const auto& pair : INPUT_PARMS_MAP) {
    if (!pair.second(prhs[toUType(pair.first)])) {
      std::stringstream error_msg;
      error_msg << "Argument " << toUType(pair.first) + 1 << " has the wrong type.\n";
      mexErrMsgIdAndTxt("teaserSolve:nargin", error_msg.str().c_str());
    }
  }

  mexPrintf("Arguments type checks passed.\n");
  mexEvalString("drawnow;");

  // Prepare parameters
  // Prepare source and destination Eigen point matrices
  Eigen::Matrix<double, 3, Eigen::Dynamic> src_eigen, dst_eigen;
  mexPointMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::src)], &src_eigen);
  mexPointMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::dst)], &dst_eigen);

  // Other parameters
  auto cbar2 = static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::cbar2)]));
  auto noise_bound = static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::noise_bound)]));
  auto estimate_scaling =
      static_cast<bool>(*mxGetPr(prhs[toUType(INPUT_PARAMS::estimate_scaling)]));
  auto rotation_estimation_method =
      static_cast<int>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_estimation_algorithm)]));
  auto rotation_gnc_factor =
      static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_gnc_factor)]));
  auto rotation_max_iterations =
      static_cast<size_t>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_max_iterations)]));
  auto rotation_cost_threshold =
      static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_cost_threshold)]));

  // Prepare the TEASER++ solver for solving registration problem
  teaser::RobustRegistrationSolver::Params params;
  params.noise_bound = noise_bound;
  params.cbar2 = cbar2;
  params.estimate_scaling = estimate_scaling;
  params.rotation_max_iterations = rotation_max_iterations;
  params.rotation_gnc_factor = rotation_gnc_factor;
  params.rotation_cost_threshold = rotation_cost_threshold;

  switch (rotation_estimation_method) {
  case 0: { // GNC-TLS method
    mexPrintf("Use GNC-TLS for rotation estimation.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
    break;
  }
  case 1: { // FGR method
    mexPrintf("Use FGR for rotation estimation.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR;
    break;
  }
  default: {
    mexPrintf("Rotation estimation method given does not exist. Use GNC-TLS instead.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
    break;
  }
  }
  teaser::RobustRegistrationSolver solver(params);

  mexPrintf("Start TEASER++ solver.\n");
  mexEvalString("drawnow;");

  // Start the timer
  auto start = std::chrono::high_resolution_clock::now();

  // Solve
  assert(src_eigen.size() != 0);
  assert(dst_eigen.size() != 0);
  solver.solve(src_eigen, dst_eigen);

  // Stop the timer
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  double duration_in_milliseconds = static_cast<double>(duration.count()) / 1000.0;

  auto solution = solver.getSolution();

  mexPrintf("TEASER++ has found a solution in %f milliseconds.\n", duration_in_milliseconds);
  mexEvalString("drawnow;");

  // Populate outputs
  plhs[toUType(OUTPUT_PARAMS::s_est)] = mxCreateDoubleScalar(solution.scale);
  // Populate output R matrix
  plhs[toUType(OUTPUT_PARAMS::R_est)] = mxCreateDoubleMatrix(3, 3, mxREAL);
  Eigen::Map<Eigen::Matrix3d> R_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::R_est)]), 3, 3);
  R_map = solution.rotation;

  // Populate output T vector
  plhs[toUType(OUTPUT_PARAMS::t_est)] = mxCreateDoubleMatrix(3, 1, mxREAL);
  Eigen::Map<Eigen::Matrix<double, 3, 1>> t_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::t_est)]), 3, 1);
  t_map = solution.translation;

  // Populate time output
  plhs[toUType(OUTPUT_PARAMS::time_taken)] = mxCreateDoubleScalar(duration_in_milliseconds);
}