//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_DECS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DECS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-arithmetic
    Function object implementing decs capabilities

    Returns the entry minus 1, saturated in the entry type.

    This is a convenient alias of @ref minusone
  **/
  const boost::dispatch::functor<tag::decs_> decs = {};
} }
#endif

#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/simd/decs.hpp>

#endif


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "modified_helmholtz_3d_hypersingular_boundary_operator.hpp"

#include "../common/boost_make_shared_fwd.hpp"

#include "abstract_boundary_operator.hpp"
#include "blas_quadrature_helper.hpp"
#include "context.hpp"
#include "general_elementary_local_operator_imp.hpp"
#include "general_elementary_singular_integral_operator_imp.hpp"
#include "general_hypersingular_integral_operator_imp.hpp"
#include "modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "synthetic_integral_operator.hpp"

#include "../fiber/explicit_instantiation.hpp"

#include "../fiber/typical_test_scalar_kernel_trial_integral.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_kernel_functor.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_kernel_interpolated_functor.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_off_diagonal_kernel_functor.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_off_diagonal_interpolated_kernel_functor.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_transformation_functor.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_transformation_functor_2.hpp"
#include "../fiber/modified_helmholtz_3d_hypersingular_integrand_functor_2.hpp"
#include "../fiber/scalar_function_value_functor.hpp"
#include "../fiber/scalar_function_value_times_normal_functor.hpp"
#include "../fiber/simple_test_scalar_kernel_trial_integrand_functor.hpp"
#include "../fiber/single_component_test_trial_integrand_functor.hpp"
#include "../fiber/surface_curl_3d_functor.hpp"

#include "../grid/max_distance.hpp"

#include <boost/type_traits/is_complex.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename KernelType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
modifiedHelmholtz3dSyntheticHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    KernelType waveNumber, std::string label, int internalSymmetry,
    bool useInterpolation, int interpPtsPerWavelength,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType> ValueFunctor;
  typedef Fiber::ScalarFunctionValueTimesNormalFunctor<CoordinateType>
  ValueTimesNormalFunctor;
  typedef Fiber::SurfaceCurl3dFunctor<CoordinateType> CurlFunctor;
  typedef Fiber::SingleComponentTestTrialIntegrandFunctor<
      BasisFunctionType, ResultType> IntegrandFunctor;

  typedef GeneralElementaryLocalOperator<BasisFunctionType, ResultType> LocalOp;
  typedef SyntheticIntegralOperator<BasisFunctionType, ResultType> SyntheticOp;

  if (!domain || !range || !dualToRange)
    throw std::invalid_argument(
        "modifiedHelmholtz3dSyntheticHypersingularBoundaryOperator(): "
        "domain, range and dualToRange must not be null");

  shared_ptr<const Space<BasisFunctionType>> newDomain = domain;
  shared_ptr<const Space<BasisFunctionType>> newDualToRange = dualToRange;

  bool isBarycentric =
      (domain->isBarycentric() || dualToRange->isBarycentric());

  if (isBarycentric) {
    newDomain = domain->barycentricSpace(domain);
    newDualToRange = dualToRange->barycentricSpace(dualToRange);
  }

  shared_ptr<const Context<BasisFunctionType, ResultType>> internalContext,
      auxContext;
  SyntheticOp::getContextsForInternalAndAuxiliaryOperators(
      context, internalContext, auxContext);
  shared_ptr<const Space<BasisFunctionType>> internalTrialSpace =
      newDomain->discontinuousSpace(newDomain);
  shared_ptr<const Space<BasisFunctionType>> internalTestSpace =
      newDualToRange->discontinuousSpace(newDualToRange);

  // Note: we don't really need to care about ranges and duals to domains of
  // the internal operator. The only range space that matters is that of the
  // leftmost operator in the product.

  const char xyz[] = "xyz";
  const size_t dimWorld = 3;

  if (label.empty())
    label =
        AbstractBoundaryOperator<BasisFunctionType, ResultType>::uniqueLabel();

  BoundaryOperator<BasisFunctionType, ResultType> slp;
  if (!externalSlp.isInitialized()) {

    slp = modifiedHelmholtz3dSingleLayerBoundaryOperator<
        BasisFunctionType, KernelType, ResultType>(
        internalContext, internalTrialSpace,
        internalTestSpace /* or whatever */, internalTestSpace, waveNumber,
        "(" + label + ")_internal_SLP", internalSymmetry, useInterpolation,
        interpPtsPerWavelength);
  } else {

    slp = externalSlp;
  }

  // symmetry of the decomposition
  int syntheseSymmetry = 0; // symmetry of the decomposition
  if (newDomain == newDualToRange && internalTrialSpace == internalTestSpace)
    syntheseSymmetry =
        HERMITIAN | (boost::is_complex<BasisFunctionType>() ? 0 : SYMMETRIC);

  std::vector<BoundaryOperator<BasisFunctionType, ResultType>> testLocalOps;
  std::vector<BoundaryOperator<BasisFunctionType, ResultType>> trialLocalOps;
  testLocalOps.resize(dimWorld);
  for (size_t i = 0; i < dimWorld; ++i)
    testLocalOps[i] = BoundaryOperator<BasisFunctionType, ResultType>(
        auxContext, boost::make_shared<LocalOp>(
                        internalTestSpace, range, newDualToRange,
                        ("(" + label + ")_test_curl_") + xyz[i], NO_SYMMETRY,
                        CurlFunctor(), ValueFunctor(), IntegrandFunctor(i, 0)));

  if (!syntheseSymmetry) {
    trialLocalOps.resize(dimWorld);
    for (size_t i = 0; i < dimWorld; ++i)
      trialLocalOps[i] = BoundaryOperator<BasisFunctionType, ResultType>(
          auxContext,
          boost::make_shared<LocalOp>(
              newDomain, internalTrialSpace /* or whatever */,
              internalTrialSpace, ("(" + label + ")_trial_curl_") + xyz[i],
              NO_SYMMETRY, ValueFunctor(), CurlFunctor(),
              IntegrandFunctor(0, i)));
  }
  // It might be more prudent to distinguish between the symmetry of the total
  // operator and the symmetry of the decomposition
  BoundaryOperator<BasisFunctionType, ResultType> term0(
      context, boost::make_shared<SyntheticOp>(testLocalOps, slp, trialLocalOps,
                                               label, syntheseSymmetry));

  for (size_t i = 0; i < dimWorld; ++i)
    testLocalOps[i] = BoundaryOperator<BasisFunctionType, ResultType>(
        auxContext,
        boost::make_shared<LocalOp>(
            internalTestSpace, range, newDualToRange,
            ("(" + label + ")_test_k_value_n_") + xyz[i], NO_SYMMETRY,
            ValueTimesNormalFunctor(), ValueFunctor(), IntegrandFunctor(i, 0)));
  if (!syntheseSymmetry)
    for (size_t i = 0; i < dimWorld; ++i)
      trialLocalOps[i] = BoundaryOperator<BasisFunctionType, ResultType>(
          auxContext,
          boost::make_shared<LocalOp>(
              newDomain, internalTrialSpace /* or whatever */,
              internalTrialSpace, ("(" + label + ")_trial_k_value_n_") + xyz[i],
              NO_SYMMETRY, ValueFunctor(), ValueTimesNormalFunctor(),
              IntegrandFunctor(0, i)));
  BoundaryOperator<BasisFunctionType, ResultType> slp_waveNumberSq =
      (waveNumber * waveNumber) * slp;
  BoundaryOperator<BasisFunctionType, ResultType> term1(
      context,
      boost::make_shared<SyntheticOp>(testLocalOps, slp_waveNumberSq,
                                      trialLocalOps, label, syntheseSymmetry));

  return term0 + term1;
}

template <typename BasisFunctionType, typename KernelType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
modifiedHelmholtz3dHypersingularBoundaryOperator(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &domain,
    const shared_ptr<const Space<BasisFunctionType>> &range,
    const shared_ptr<const Space<BasisFunctionType>> &dualToRange,
    KernelType waveNumber, const std::string &label, int symmetry,
    bool useInterpolation, int interpPtsPerWavelength,
    const BoundaryOperator<BasisFunctionType, ResultType> &externalSlp) {
  const AssemblyOptions &assemblyOptions = context->assemblyOptions();
  if ((assemblyOptions.assemblyMode() == AssemblyOptions::ACA &&
       assemblyOptions.acaOptions().mode == AcaOptions::LOCAL_ASSEMBLY) ||
      externalSlp.isInitialized())
    return modifiedHelmholtz3dSyntheticHypersingularBoundaryOperator(
        context, domain, range, dualToRange, waveNumber, label, symmetry,
        useInterpolation, interpPtsPerWavelength, externalSlp);

  typedef typename ScalarTraits<BasisFunctionType>::RealType CoordinateType;

  typedef Fiber::ModifiedHelmholtz3dHypersingularKernelFunctor<KernelType>
  NoninterpolatedKernelFunctor;
  typedef Fiber::ModifiedHelmholtz3dHypersingularKernelInterpolatedFunctor<
      KernelType> InterpolatedKernelFunctor;
  typedef Fiber::ModifiedHelmholtz3dHypersingularTransformationFunctor<
      CoordinateType> TransformationFunctor;
  typedef Fiber::ModifiedHelmholtz3dHypersingularIntegrandFunctor2<
      BasisFunctionType, KernelType, ResultType> IntegrandFunctor;

  typedef Fiber::ModifiedHelmholtz3dHypersingularTransformationFunctor2<
      CoordinateType> TransformationFunctorWithBlas;

  typedef Fiber::
      ModifiedHelmholtz3dHypersingularOffDiagonalInterpolatedKernelFunctor<
          KernelType> OffDiagonalInterpolatedKernelFunctor;
  typedef Fiber::ModifiedHelmholtz3dHypersingularOffDiagonalKernelFunctor<
      KernelType> OffDiagonalNoninterpolatedKernelFunctor;
  typedef Fiber::ScalarFunctionValueFunctor<CoordinateType>
  OffDiagonalTransformationFunctor;
  typedef Fiber::SimpleTestScalarKernelTrialIntegrandFunctorExt<
      BasisFunctionType, KernelType, ResultType, 1> OffDiagonalIntegrandFunctor;

  CoordinateType maxDistance_ =
      static_cast<CoordinateType>(1.1) *
      maxDistance(*domain->grid(), *dualToRange->grid());

  shared_ptr<Fiber::TestKernelTrialIntegral<BasisFunctionType, KernelType,
                                            ResultType>> integral,
      offDiagonalIntegral;
  if (shouldUseBlasInQuadrature(assemblyOptions, *domain, *dualToRange)) {
    integral.reset(new Fiber::TypicalTestScalarKernelTrialIntegral<
        BasisFunctionType, KernelType, ResultType>());
    offDiagonalIntegral = integral;
  } else {
    integral.reset(new Fiber::DefaultTestKernelTrialIntegral<IntegrandFunctor>(
        IntegrandFunctor()));
    offDiagonalIntegral.reset(
        new Fiber::DefaultTestKernelTrialIntegral<OffDiagonalIntegrandFunctor>(
            OffDiagonalIntegrandFunctor()));
  }

  typedef GeneralHypersingularIntegralOperator<BasisFunctionType, KernelType,
                                               ResultType> Op;
  shared_ptr<Op> newOp;
  if (shouldUseBlasInQuadrature(assemblyOptions, *domain, *dualToRange)) {
    shared_ptr<Fiber::TestKernelTrialIntegral<BasisFunctionType, KernelType,
                                              ResultType>> integral,
        offDiagonalIntegral;
    integral.reset(new Fiber::TypicalTestScalarKernelTrialIntegral<
        BasisFunctionType, KernelType, ResultType>());
    offDiagonalIntegral = integral;
    if (useInterpolation)
      newOp.reset(new Op(
          domain, range, dualToRange, label, symmetry,
          InterpolatedKernelFunctor(waveNumber, maxDistance_,
                                    interpPtsPerWavelength),
          TransformationFunctorWithBlas(), TransformationFunctorWithBlas(),
          integral, OffDiagonalInterpolatedKernelFunctor(
                        waveNumber, maxDistance_, interpPtsPerWavelength),
          OffDiagonalTransformationFunctor(),
          OffDiagonalTransformationFunctor(), offDiagonalIntegral));
    else
      newOp.reset(new Op(
          domain, range, dualToRange, label, symmetry,
          NoninterpolatedKernelFunctor(waveNumber),
          TransformationFunctorWithBlas(), TransformationFunctorWithBlas(),
          integral, OffDiagonalNoninterpolatedKernelFunctor(waveNumber),
          OffDiagonalTransformationFunctor(),
          OffDiagonalTransformationFunctor(), offDiagonalIntegral));
  } else { // no blas
    if (useInterpolation)
      newOp.reset(new Op(
          domain, range, dualToRange, label, symmetry,
          InterpolatedKernelFunctor(waveNumber, maxDistance_,
                                    interpPtsPerWavelength),
          TransformationFunctor(), TransformationFunctor(), IntegrandFunctor(),
          OffDiagonalInterpolatedKernelFunctor(waveNumber, maxDistance_,
                                               interpPtsPerWavelength),
          OffDiagonalTransformationFunctor(),
          OffDiagonalTransformationFunctor(), OffDiagonalIntegrandFunctor()));
    else
      newOp.reset(new Op(
          domain, range, dualToRange, label, symmetry,
          NoninterpolatedKernelFunctor(waveNumber), TransformationFunctor(),
          TransformationFunctor(), IntegrandFunctor(),
          OffDiagonalNoninterpolatedKernelFunctor(waveNumber),
          OffDiagonalTransformationFunctor(),
          OffDiagonalTransformationFunctor(), OffDiagonalIntegrandFunctor()));
  }
  return BoundaryOperator<BasisFunctionType, ResultType>(context, newOp);
}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, KERNEL, RESULT)               \
  template BoundaryOperator<BASIS, RESULT>                                     \
  modifiedHelmholtz3dHypersingularBoundaryOperator(                            \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, KERNEL, const std::string &,     \
      int, bool, int, const BoundaryOperator<BASIS, RESULT> &externalSlp)

FIBER_ITERATE_OVER_BASIS_KERNEL_AND_RESULT_TYPES(
    INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // namespace Bempp


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <ros/ros.h>

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

#include "MPC.h"
#include "Trajectory.h"
#include "Path.h"

#include <tf/tf.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <kugle_msgs/StateEstimate.h>
#include <std_srvs/Empty.h>

#include <boost/math/quaternion.hpp> // see https://www.boost.org/doc/libs/1_66_0/libs/math/doc/html/quaternions.html
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

/* For plotting/visualization */
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

boost::math::quaternion<double> Quaternion_eul2quat_zyx(const float yaw, const float pitch, const float roll);
void Quaternion_quat2eul_zyx(boost::math::quaternion<double> q, float yaw_pitch_roll[3]);
void OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg);
void StateEstimateCallback(const kugle_msgs::StateEstimate::ConstPtr& msg);
void GetLocalizationBasedPositionEstimate(tf::TransformListener * tfListener, std::string map_frame, std::string base_link_frame);
void PublishTrajectory(const MPC::Trajectory &trajectory, ros::Publisher &pub, std::string mapFrame);
void PublishPredictedHorizon(MPC::MPC &mpc, ros::Publisher &pub, std::string mapFrame);
void GazeboReset(ros::NodeHandle &n);
void GazeboPause(ros::NodeHandle &n);
void GazeboResume(ros::NodeHandle &n);

nav_msgs::Odometry odom;
kugle_msgs::StateEstimate stateEstimate;
struct {
    double time;
    Eigen::Vector2d position;
    boost::math::quaternion<double> quaternion;
} localizationEstimate;

int main(int argc, char **argv) {
	std::string nodeName = "mpc_test";
	ros::init(argc, argv, nodeName.c_str());
	ros::NodeHandle n;
    ros::NodeHandle nParam("~"); // default/current namespace node handle
    geometry_msgs::Twist cmd_vel;

	double rate = 10;
    if (!nParam.getParam("rate", rate)) {
        ROS_WARN_STREAM("Loop rate not set (Parameter: rate). Defaults to: 10 Hz");
    }

    std::string map_frame = "map";
    if (!nParam.getParam("map_frame", map_frame)) {
        ROS_WARN_STREAM("Map frame not set (Parameter: map_frame). Defaults to: map");
    }

    std::string base_link_frame = "base_link";
    if (!nParam.getParam("base_link_frame", base_link_frame)) {
        ROS_WARN_STREAM("base_link frame not set (Parameter: base_link_frame). Defaults to: base_link");
    }

    bool use_localization = false;
    if (!nParam.getParam("use_localization", use_localization)) {
        ROS_WARN_STREAM("Use localization flag not set (Parameter: use_localization). Defaults to: false");
    }

    double desired_velocity = 1.0;
    if (!nParam.getParam("desired_velocity", use_localization)) {
        ROS_WARN_STREAM("Desired velocity not set (Parameter: desired_velocity). Defaults to: 1.0 m/s");
    }

    ros::Rate loop_rate(rate); // Loop rate
    ros::Publisher pub_cmd_vel_angular_inertial = n.advertise<geometry_msgs::Twist>("cmd_vel_inertial", 1000);
    ros::Publisher pub_trajectory = n.advertise<nav_msgs::Path>("/KugleMPC/global_plan", 1000); // publish under KugleMPC to make consistent with kugle_mpc_plugin
    ros::Publisher pub_horizon = n.advertise<nav_msgs::Path>("/KugleMPC/local_plan", 1000);
    ros::Subscriber sub_odom = n.subscribe("odom", 1000, &OdometryCallback);
    ros::Subscriber sub_stateestimate = n.subscribe("StateEstimate", 1000, &StateEstimateCallback);
    tf::TransformListener tfListener;

    // Wait for first state estimate message
    stateEstimate.mcu_time = 0;
    localizationEstimate.time = 0;
    while (ros::ok())
    {
        if (stateEstimate.mcu_time > 0 && (localizationEstimate.time > 0 || !use_localization)) break;

        if (use_localization)
            GetLocalizationBasedPositionEstimate(&tfListener, map_frame, base_link_frame);

        if (use_localization)
            std::cout << "Waiting for localization" << std::endl;
        else
            std::cout << "Waiting for State Estimate message" << std::endl;

        /*std::cout << "Position estimate" << std::endl;
        std::cout << "odom = " << std::endl << odom.pose.pose.position;
        std::cout << "stateEst = " << std::endl << "(" << stateEstimate.position[0] << ", " << stateEstimate.position[1] << ")" << std::endl;
        std::cout << "tf = " << std::endl << localizationEstimate.position << std::endl;
        std::cout << std::endl;*/

        ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
        loop_rate.sleep();
    }

	MPC::MPC mpc;
    mpc.setDesiredVelocity(desired_velocity);

    boost::math::quaternion<double> RobotQuaternion;
    Eigen::Vector2d RobotPos;
    Eigen::Vector2d RobotVelocity; // inertial frame
    MPC::MPC::state_t state;

    RobotQuaternion = boost::math::quaternion<double>(stateEstimate.q.w, stateEstimate.q.x, stateEstimate.q.y, stateEstimate.q.z);
    if (use_localization) {
        RobotPos(0) = localizationEstimate.position[0];
        RobotPos(1) = localizationEstimate.position[1];
    } else {
        RobotPos(0) = stateEstimate.position[0];
        RobotPos(1) = stateEstimate.position[1];
    }
    RobotVelocity(0) = stateEstimate.velocity[0];
    RobotVelocity(1) = stateEstimate.velocity[1];

    MPC::Trajectory trajectory = MPC::Trajectory::GenerateCircleTrajectory(RobotPos);
	mpc.setTrajectory(trajectory, RobotPos, Eigen::Vector2d(0.01,0), RobotQuaternion);
	//mpc.setXYreferencePosition(RobotPos(0), RobotPos(1));
	mpc.setCurrentState(RobotPos, Eigen::Vector2d(0.01,0), RobotQuaternion);
	//mpc.setObstacles(obstacles);

	while (ros::ok())
	{
		/*if ((i % 10) == 0) {
		    mpc.setTrajectory(trajectory, state.position, state.velocity, state.quaternion);
		    mpc.setObstacles(obstacles);
		    //mpc.setObstacles(obstacles);
		}*/

        if (use_localization)
            GetLocalizationBasedPositionEstimate(&tfListener, map_frame, base_link_frame);

        RobotQuaternion = boost::math::quaternion<double>(stateEstimate.q.w, stateEstimate.q.x, stateEstimate.q.y, stateEstimate.q.z);
        if (use_localization) {
            RobotPos(0) = localizationEstimate.position[0];
            RobotPos(1) = localizationEstimate.position[1];
        } else {
            RobotPos(0) = stateEstimate.position[0];
            RobotPos(1) = stateEstimate.position[1];
        }
        RobotVelocity(0) = stateEstimate.velocity[0];
        RobotVelocity(1) = stateEstimate.velocity[1];

		mpc.setTrajectory(trajectory, RobotPos, RobotVelocity, RobotQuaternion);
		mpc.setCurrentState(RobotPos, RobotVelocity, RobotQuaternion);

		PublishTrajectory(trajectory, pub_trajectory, map_frame);

		cv::Mat imgTrajectory = cv::Mat( 500, 1333, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		trajectory.plot(imgTrajectory, cv::Scalar(0, 0, 255), false, false, -8, -3, 8, 3);
		mpc.PlotRobot(imgTrajectory, cv::Scalar(255, 0, 0), false, -8, -3, 8, 3);
		mpc.PlotObstacles(imgTrajectory, cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), false, -8, -3, 8, 3);
		//cv::imshow("Trajectory", imgTrajectory);

		cv::Mat imgWindowTrajectory = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.getCurrentTrajectory().plot(imgWindowTrajectory, cv::Scalar(0, 255, 0), true, false, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgWindowTrajectory, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgWindowTrajectory, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Window", imgWindowTrajectory);

		cv::Mat imgWindowPath = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.getCurrentPath().plot(imgWindowPath, cv::Scalar(0, 255, 0), true, -4, -4, 4, 4);
		mpc.getCurrentPath().PlotPoint(mpc.getClosestPointOnPath(), imgWindowPath, cv::Scalar(0, 0, 255), true, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgWindowPath, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgWindowPath, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Path", imgWindowPath);

		mpc.Step();

		Eigen::Vector2d angularVelocityReference(0.0, 0.0);
		if (mpc.getStatus() != MPC::MPC::SUCCESS) {
		    std::cout << "MPC Solver failed" << std::endl;
            GazeboPause(n);
		    cv::waitKey(0);
            GazeboResume(n);
		}

        PublishPredictedHorizon(mpc, pub_horizon, map_frame);

		state = mpc.getHorizonState();
		std::cout << "Predicted states:" << std::endl;
		std::cout << "path distance = " << state.pathDistance << std::endl;
		std::cout << "position = " << std::endl << state.position << std::endl;
		std::cout << "velocity = " << std::endl << state.velocity << std::endl;
		std::cout << "quaternion = " << std::endl << state.quaternion << std::endl;
		std::cout << std::endl;

		angularVelocityReference = mpc.getInertialAngularVelocity();
		std::cout << "Control output (angular velocity):" << std::endl;
		std::cout << "   x = " << angularVelocityReference[0] << std::endl;
		std::cout << "   y = " << angularVelocityReference[1] << std::endl;

		cv::Mat imgPredicted = cv::Mat( 500, 500, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
		mpc.PlotPredictedTrajectoryInWindow(imgPredicted, -4, -4, 4, 4);
		mpc.PlotObstaclesInWindow(imgPredicted, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		mpc.PlotRobotInWindow(imgPredicted, cv::Scalar(255, 0, 0), true, -4, -4, 4, 4);
		cv::imshow("Predicted", imgPredicted);

        mpc.PlotPredictedTrajectory(imgTrajectory, -8, -3, 8, 3);
        cv::imshow("Trajectory", imgTrajectory);

        cmd_vel.angular.x = angularVelocityReference[0];
        cmd_vel.angular.y = angularVelocityReference[1];
        cmd_vel.angular.z = 0;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.linear.z = 0;

        pub_cmd_vel_angular_inertial.publish(cmd_vel);

        cv::waitKey(1);
		ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
		loop_rate.sleep();
	}
}

void OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received odometry, translational velocity: (" << msg->twist.twist.linear.x << ", " << msg->twist.twist.linear.y << ")");
    odom = *msg;
}

void StateEstimateCallback(const kugle_msgs::StateEstimate::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received StateEstimate message");
    stateEstimate = *msg;
}

void GetLocalizationBasedPositionEstimate(tf::TransformListener * tfListener, std::string map_frame, std::string base_link_frame)
{
    ros::Time currentTime = ros::Time::now();

    // Look-up current estimate of robot in map
    tf::StampedTransform tf_base_link;
    try {
        tfListener->lookupTransform(map_frame, base_link_frame, ros::Time(0), tf_base_link);

        Eigen::Vector2d robot_center(tf_base_link.getOrigin().x(), tf_base_link.getOrigin().y());
        tf::Quaternion q = tf_base_link.getRotation();

        localizationEstimate.quaternion =  boost::math::quaternion<double>(q.w(), q.x(), q.y(), q.z());
        localizationEstimate.position = robot_center;
        localizationEstimate.time = currentTime.toSec();
    }
    catch (tf::TransformException &ex) {
        ROS_WARN("%s", ex.what());
    }
}

void PublishTrajectory(const MPC::Trajectory &trajectory, ros::Publisher &pub, std::string mapFrame)
{
    nav_msgs::Path planMsg;
    planMsg.header.stamp = ros::Time::now();
    planMsg.header.frame_id = mapFrame;

    // Populate vector of trajectory poses
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = planMsg.header.stamp;
    pose.header.frame_id = planMsg.header.frame_id;
    pose.pose.orientation.w = 1.0; // set unit quaternion (upright) for reference poses
    pose.pose.orientation.x = 0.0;
    pose.pose.orientation.y = 0.0;
    pose.pose.orientation.z = 0.0;
    pose.pose.position.z = 0;
    for (unsigned int i = 0; i < trajectory.size(); i++) {
        auto point = trajectory.get(i);
        pose.header.seq = i;
        pose.pose.position.x = point.point[0];
        pose.pose.position.y = point.point[1];
        planMsg.poses.push_back(pose);
    }

    // Publish path message included the trajectory poses
    pub.publish(planMsg);
}

void PublishPredictedHorizon(MPC::MPC &mpc, ros::Publisher &pub, std::string mapFrame)
{
    nav_msgs::Path planMsg;
    planMsg.header.stamp = ros::Time::now();
    planMsg.header.frame_id = mapFrame;

    for (unsigned int i = 0; i < MPC::MPC::HorizonLength; i++) {
        auto state = mpc.getHorizonState(i);

        geometry_msgs::PoseStamped pose;
        pose.header.seq = i;
        pose.header.stamp = planMsg.header.stamp;
        pose.header.frame_id = planMsg.header.frame_id;
        pose.pose.position.x = state.position[0];
        pose.pose.position.y = state.position[1];
        pose.pose.position.z = 0;
        pose.pose.orientation.w = state.quaternion.R_component_1();
        pose.pose.orientation.x = state.quaternion.R_component_2();
        pose.pose.orientation.y = state.quaternion.R_component_3();
        pose.pose.orientation.z = state.quaternion.R_component_4();

        planMsg.poses.push_back(pose);
    }

    pub.publish(planMsg);
}

void GazeboReset(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/reset_world");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/reset_world");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/reset_world");
    }
}

void GazeboPause(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/pause_physics");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/pause_physics");
    }
}

void GazeboResume(ros::NodeHandle &n)
{
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
    std_srvs::Empty srv;
    if (client.call(srv))
    {
        ROS_INFO("Success calling /gazebo/unpause_physics");
    }
    else
    {
        ROS_ERROR("Failed calling /gazebo/unpause_physics");
    }
}

boost::math::quaternion<double> Quaternion_eul2quat_zyx(const float yaw, const float pitch, const float roll)
{
	const float cx = cosf(roll/2);
	const float cy = cosf(pitch/2);
	const float cz = cosf(yaw/2);
	const float sx = sinf(roll/2);
	const float sy = sinf(pitch/2);
	const float sz = sinf(yaw/2);

	double q[4];
	q[0] = cz*cy*cx+sz*sy*sx;
	q[1] = cz*cy*sx-sz*sy*cx;
	q[2] = cz*sy*cx+sz*cy*sx;
	q[3] = sz*cy*cx-cz*sy*sx;

	return boost::math::quaternion<double>(q[0], q[1], q[2], q[3]);
}

void Quaternion_quat2eul_zyx(boost::math::quaternion<double> q, float yaw_pitch_roll[3])
{
	// Normalize quaternion
	q /= norm(q);

	float qw = q.R_component_1();
	float qx = q.R_component_2();
	float qy = q.R_component_3();
	float qz = q.R_component_4();

	float aSinInput = -2*(qx*qz-qw*qy);
	aSinInput = fmax(fmin(aSinInput, 1.f), -1.f);

	yaw_pitch_roll[0] = atan2( 2*(qx*qy+qw*qz), qw*qw + qx*qx - qy*qy - qz*qz ); // yaw
	yaw_pitch_roll[1] = asin( aSinInput ); // pitch
	yaw_pitch_roll[2] = atan2( 2*(qy*qz+qw*qx), qw*qw - qx*qx - qy*qy + qz*qz ); // roll
}


// ROS specific includes
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>

// PCL specific includes

// PCL feature includes
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/integral_image_normal.h>

// PCL comparators
#include <pcl/segmentation/comparator.h>
#include <pcl/segmentation/plane_coefficient_comparator.h>
#include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
#include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
#include <pcl/segmentation/edge_aware_plane_comparator.h>

#include <pcl/tracking/tracking.h>
#include <pcl/tracking/particle_filter.h>
#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
#include <pcl/tracking/particle_filter_omp.h>

#include <pcl/tracking/coherence.h>
#include <pcl/tracking/distance_coherence.h>
#include <pcl/tracking/hsv_color_coherence.h>
#include <pcl/tracking/normal_coherence.h>

#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
#include <pcl/common/centroid.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/io/pcd_io.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/integral_image_normal.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/segmentation/euclidean_cluster_comparator.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/organized_connected_component_segmentation.h>

#include <pcl/surface/convex_hull.h>

#include <pcl/search/pcl_search.h>
#include <pcl/common/transforms.h>

#include <boost/format.hpp>

// Macros
#define FPS_CALC_BEGIN                          \
    static double duration = 0;                 \
    double start_time = pcl::getTime ();        \

#define FPS_CALC_END(_WHAT_)                    \
  {                                             \
    double end_time = pcl::getTime ();          \
    static unsigned count = 0;                  \
    if (++count == 10)                          \
    {                                           \
      std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(duration) << " Hz" <<  std::endl; \
      count = 0;                                                        \
      duration = 0.0;                                                   \
    }                                           \
    else                                        \
    {                                           \
      duration += end_time - start_time;        \
    }                                           \
}
// ---------------------------------

using namespace pcl::tracking;

template <typename PointType>
class ObjectViewer
{
public:

  // Typedefs
  typedef pcl::PointXYZRGBA RefPointType;
  typedef ParticleXYZRPY ParticleT;

  typedef pcl::PointCloud<PointType> Cloud;
  typedef pcl::PointCloud<RefPointType> RefCloud;
  typedef typename RefCloud::Ptr RefCloudPtr;
  typedef typename RefCloud::ConstPtr RefCloudConstPtr;
  typedef typename Cloud::Ptr CloudPtr;
  typedef typename Cloud::ConstPtr CloudConstPtr;
  typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;
  typedef typename ParticleFilter::CoherencePtr CoherencePtr;
  typedef typename pcl::search::KdTree<PointType> KdTree;
  typedef typename KdTree::Ptr KdTreePtr;

  ObjectViewer (ros::NodeHandle nh)
  : nh_(nh)
  , viewer_ ("PCL Object Viewer")
  , cloud_ (new Cloud)
  , cloud_ref_ (new Cloud)
  , new_cloud_ (false)
  , have_clusters_ (false)
  , clusters_size_ (0)
  , factor_ (0.02f)
  , size_ (20.0f)
  , planeMinSize_ (10000)
  , clusterMinSize_ (500)
  , angularTh_  (2.0f)
  , distanceTh_ (0.01f)
  {

    // Init the PCL viewer
    viewer_.runOnVisualizationThread ( boost::bind( &ObjectViewer::viz_cb, this, _1), "viz_cb");
    viewer_.registerKeyboardCallback ( &ObjectViewer::keyboard_callback, *this, 0);

    // Create a ROS subscriber for the input point cloud
    sub_ = nh_.subscribe ("cloud", 1, &ObjectViewer::cloud_cb, this);

    // Create comparator objects
    plane_comparator_.reset (new pcl::PlaneCoefficientComparator<PointType, pcl::Normal> ());
    euclidean_comparator_.reset (new pcl::EuclideanPlaneCoefficientComparator<PointType, pcl::Normal> ());
    rgb_comparator_.reset (new pcl::RGBPlaneCoefficientComparator<PointType, pcl::Normal> ());
    edge_aware_comparator_.reset (new pcl::EdgeAwarePlaneComparator<PointType, pcl::Normal> ());
    euclidean_cluster_comparator_.reset (new pcl::EuclideanClusterComparator<PointType, pcl::Normal, pcl::Label> ());

    // Set default comparator used for refinement
    mps_.setComparator (edge_aware_comparator_);
  }

  // -------------------------
  // PCL visualization functions
  // ------------------------------------

  void
  drawParticles (pcl::visualization::PCLVisualizer& viz)
  {
    char name[1024];
    for (size_t i = 0; i < trackers_.size(); i++) {
      sprintf (name, "cluster_particles_%d" , int (i));
      ParticleFilter::PointCloudStatePtr particles = trackers_[i]->getParticles ();
      if (particles) {

        pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
        for (size_t i = 0; i < particles->points.size (); i++) {
          pcl::PointXYZ point;

          point.x = particles->points[i].x;
          point.y = particles->points[i].y;
          point.z = particles->points[i].z;
          particle_cloud->points.push_back (point);
	}

        {
          pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
          if (!viz.updatePointCloud (particle_cloud, blue_color, name))
            viz.addPointCloud (particle_cloud, blue_color, name);
        }
      }
      /*
      else {
	PCL_WARN("no particles");
      }
      */
    }
  }

  void
  drawResult (pcl::visualization::PCLVisualizer& viz)
  {
    char name[1024];
    for (size_t i = 0; i < trackers_.size(); i++) {

      sprintf (name, "cluster_%d" , int (i));
      ParticleXYZRPY result = trackers_[i]->getResult ();
      Eigen::Affine3f transformation = trackers_[i]->toEigenMatrix (result);
      // move a little bit for better visualization
      //transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
      RefCloudPtr result_cloud (new RefCloud ());
      pcl::transformPointCloud<RefPointType> ( *references_[i], *result_cloud, transformation);
      {
	pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
	if (!viz.updatePointCloud (result_cloud, red_color, name))
	  viz.addPointCloud (result_cloud, red_color, name);
      }
    }
  }

  void
  removeClusters (size_t clusters_size, pcl::visualization::PCLVisualizer& viz)
  {
    char name[1024];
    for (size_t i = 0; i < clusters_size; i++) {
      sprintf (name, "cluster_%d", int (i));
      viz.removePointCloud (name);
    }
  }

  void
  displayClusters (const pcl::PointCloud<RefPointType>::CloudVectorType &clusters,
		   pcl::visualization::PCLVisualizer& viz)
  {
    char name[1024];
    unsigned char red [6] = {255,   0,   0, 255, 255,   0};
    unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
    unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};

    for (size_t i = 0; i < clusters.size (); i++) {
      sprintf (name, "cluster_%d" , int (i));
      pcl::visualization::PointCloudColorHandlerCustom<PointType> color0(boost::make_shared<pcl::PointCloud<PointType> >(clusters[i]),red[i%6],grn[i%6],blu[i%6]);
      if (!viz.updatePointCloud (boost::make_shared<pcl::PointCloud<PointType> >(clusters[i]), color0, name))
	viz.addPointCloud (boost::make_shared<pcl::PointCloud<PointType> >(clusters[i]), color0, name);
      viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
      viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, name);
    }
  }

  void
  viz_cb (pcl::visualization::PCLVisualizer& viz)
  {
    boost::mutex::scoped_lock lock (mtx_);

    if (new_cloud_)
    {

      CloudPtr cloud = cloud_;
      if (!viz.updatePointCloud (cloud, "object_cloud")) {
	viz.addPointCloud (cloud, "object_cloud");
	viz.resetCameraViewpoint ("object_cloud");
      }

      if (have_clusters_) {
	drawParticles (viz);
	drawResult (viz);
      }

      /*
      removeClusters(clusters_size_, viz);
      displayClusters( clusters_, viz);
      clusters_size_ = clusters_.size();
      */
    }
    new_cloud_ = false;
  }

  void
  keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
  {
    if (event.keyUp ()) {
      std::cout << "key: " << event.getKeyCode () << std::endl;
      switch (event.getKeyCode ()) {
      case '1':
	std::cout << "Using [plane comparator]... " << std::endl;
	mps_.setComparator (plane_comparator_);
	break;
      case '2':
	std::cout << "Using [euclidean comparator]... " << std::endl;
	mps_.setComparator (euclidean_comparator_);
	break;
      case '3':
	std::cout << "Using [rgb comparator]... " << std::endl;
	mps_.setComparator (rgb_comparator_);
	break;
      case '4':
	std::cout << "Using [edge aware comparator]... " << std::endl;
	mps_.setComparator (edge_aware_comparator_);
	break;
      case '5':
	factor_  -= 0.01f;
	std::cout << "factor: " << factor_ << std::endl;
	break;
      case '6':
	factor_  += 0.01f;
	std::cout << "factor: " << factor_ << std::endl;
	break;
      case '7':
	size_  -= 1.0f;
	std::cout << "size: " << size_ << std::endl;
	break;
      case '8':
	size_  += 1.0f;
	std::cout << "size: " << size_ << std::endl;
	break;
      case '9':
	planeMinSize_  -= 1000;
	std::cout << "planeMinSize: " << planeMinSize_ << std::endl;
	break;
      case '0':
	planeMinSize_  += 1000;
	std::cout << "planeMinSize: " << planeMinSize_ << std::endl;
	break;
      case 'v':
	clusterMinSize_  -= 100;
	std::cout << "clusterMinSize: " << clusterMinSize_ << std::endl;
	break;
      case 'b':
	clusterMinSize_  += 100;
	std::cout << "clusterMinSize: " << clusterMinSize_ << std::endl;
	break;
      case 'n':
	angularTh_ -= 0.1f;
	std::cout << "angularTh: " << angularTh_ << std::endl;
	break;
      case 'm':
	angularTh_ += 0.1f;
	std::cout << "angularTh: " << angularTh_ << std::endl;
	break;
      case ',':
	distanceTh_ -= 0.01f;
	std::cout << "distanceTh: " << distanceTh_ << std::endl;
	break;
      case '.':
	distanceTh_ += 0.01f;
	std::cout << "distanceTh: " << distanceTh_ << std::endl;
	break;
      }
    }
  }

  // --------------------------------
  // Tracking functions
  // ----------------------------------------------

  void
  initTrackers(size_t clusters_size) {
    trackers_.clear();

    std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
    default_step_covariance[3] *= 40.0;
    default_step_covariance[4] *= 40.0;
    default_step_covariance[5] *= 40.0;

    std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
    std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);

    // Init a list of tracker objects
    for(size_t i = 0; i < clusters_size; i++) {
      boost::shared_ptr<ParticleFilterOMPTracker<RefPointType, ParticleT> > tracker (new ParticleFilterOMPTracker<RefPointType, ParticleT> (8));

      tracker->setTrans (Eigen::Affine3f::Identity ());
      tracker->setStepNoiseCovariance (default_step_covariance);
      tracker->setInitialNoiseCovariance (initial_noise_covariance);
      tracker->setInitialNoiseMean (default_initial_mean);
      tracker->setIterationNum (1);
      tracker->setParticleNum (250);
      tracker->setResampleLikelihoodThr(0.00);
      tracker->setUseNormal (false);

      // Setup coherences
      ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
	(new ApproxNearestPairPointCloudCoherence<RefPointType> ());

      boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
	= boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
      coherence->addPointCoherence (distance_coherence);

      boost::shared_ptr<HSVColorCoherence<RefPointType> > color_coherence
	= boost::shared_ptr<HSVColorCoherence<RefPointType> > (new HSVColorCoherence<RefPointType> ());
      color_coherence->setWeight (0.1);
      coherence->addPointCoherence (color_coherence);

      // Search method used
      //boost::shared_ptr<pcl::search::KdTree<RefPointType> > search (new pcl::search::KdTree<RefPointType> (false));
      //boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
      boost::shared_ptr<pcl::search::OrganizedNeighbor<RefPointType> > search (new pcl::search::OrganizedNeighbor<RefPointType>);

      coherence->setSearchMethod (search);
      coherence->setMaximumDistance (0.01);
      tracker->setCloudCoherence (coherence);

      trackers_.push_back(tracker);
    }
  }

  void
  tracking (const RefCloudConstPtr &cloud_ptr)
  {
    //double start = pcl::getTime ();
    FPS_CALC_BEGIN;
    for (size_t i = 0; i < trackers_.size(); i++) {
      trackers_[i]->setInputCloud (cloud_ptr);
      trackers_[i]->compute ();
    }
    //double end = pcl::getTime ();
    FPS_CALC_END("tracking");
    //tracking_time_ = end - start;
  }

  // --------------------------------
  // Filter & segmentation functions
  // ----------------------------------------------

  void
  filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
  {
    FPS_CALC_BEGIN;
    pcl::PassThrough<PointType> pass;
    pass.setFilterFieldName ("z");
    //pass.setFilterLimits (0.0, 10.0);
    pass.setFilterLimits (0.0, 1.5);
    //pass.setFilterLimits (0.0, 0.6);
    pass.setKeepOrganized (true);
    pass.setInputCloud (cloud);
    pass.filter (result);
    FPS_CALC_END("filterPassThrough");
  }

  void
  gridSample (const CloudConstPtr &cloud, Cloud &result, bool use_approx = true, double leaf_size = 0.02)
  {
    FPS_CALC_BEGIN;
    //double start = pcl::getTime ();
    if( use_approx ) {
      pcl::ApproximateVoxelGrid<PointType> grid;
      grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
      grid.setInputCloud (cloud);
      grid.filter (result);
    }
    else {
      pcl::VoxelGrid<PointType> grid;
      grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
      grid.setInputCloud (cloud);
      grid.filter (result);
    }
    //double end = pcl::getTime ();
    //downsampling_time_ = end - start;
    FPS_CALC_END("gridSample");
  }

  void
  removeZeroPoints (const Cloud &cloud,
		    Cloud &result)
  {
    for (size_t i = 0; i < cloud.points.size (); i++) {
      PointType point = cloud.points[i];
      if (!(fabs(point.x) < 0.01 &&
            fabs(point.y) < 0.01 &&
            fabs(point.z) < 0.01) &&
          !pcl_isnan(point.x) &&
          !pcl_isnan(point.y) &&
          !pcl_isnan(point.z))
        result.points.push_back(point);
    }
  }

  void integralEstimate (const CloudConstPtr &cloud_ptr,
			 pcl::PointCloud<pcl::Normal>::Ptr &normals_ptr,
			 double factor = 0.02f, // 0.03f
			 double size = 20.0f,   // 10.0
			 double distanceTh = 0.01f )
  {
    FPS_CALC_BEGIN;
    nii_.setNormalEstimationMethod (nii_.COVARIANCE_MATRIX); // nii_.AVERAGE_3D_GRADIENT || nii_.COVARIANCE_MATRIX
    nii_.setMaxDepthChangeFactor (factor);
    //nii_.setDepthDependentSmoothing (false);
    nii_.setNormalSmoothingSize (size);
    nii_.setInputCloud (cloud_ptr);
    nii_.compute (*normals_ptr);

    float* distance_map = nii_.getDistanceMap ();
    boost::shared_ptr<pcl::EdgeAwarePlaneComparator<PointType,pcl::Normal> > eapc = boost::dynamic_pointer_cast<pcl::EdgeAwarePlaneComparator<PointType,pcl::Normal> >(edge_aware_comparator_);
    eapc->setDistanceMap (distance_map);
    eapc->setDistanceThreshold (distanceTh, false);
    FPS_CALC_END("integralEstimate");
  }

  void segmentOrganized (const CloudConstPtr &cloud_ptr,
			 const pcl::PointCloud<pcl::Normal>::Ptr &normals_ptr,
			 pcl::PointCloud<RefPointType>::CloudVectorType &clusters,
			 int planeMinSize = 10000,
			 int clusterMinSize = 500,
			 double angularTh = 3.0f,     // degrees (3.0f)
			 double distanceTh = 0.02f )  // (0.02f)
  {
    // Output data structures (use in subsequent processing)
    std::vector<pcl::PlanarRegion<PointType>, Eigen::aligned_allocator<pcl::PlanarRegion<PointType> > > regions;
    std::vector<pcl::ModelCoefficients> model_coefficients;
    std::vector<pcl::PointIndices> inlier_indices;
    pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
    std::vector<pcl::PointIndices> label_indices;
    std::vector<pcl::PointIndices> boundary_indices;

    // Segment planes
    FPS_CALC_BEGIN;
    mps_.setMinInliers (planeMinSize);
    mps_.setAngularThreshold (pcl::deg2rad (angularTh));
    mps_.setDistanceThreshold (distanceTh);
    mps_.setInputNormals (normals_ptr);
    mps_.setInputCloud (cloud_ptr);
    mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);

    // Segment objects
    if (regions.size () > 0) {
      std::vector<bool> plane_labels;
      plane_labels.resize (label_indices.size (), false);
      for (size_t i = 0; i < label_indices.size (); i++) {
	if ( (int)label_indices[i].indices.size () > planeMinSize ) {
	  plane_labels[i] = true;
	}
      }

      euclidean_cluster_comparator_->setInputCloud (cloud_ptr);
      euclidean_cluster_comparator_->setLabels (labels);
      euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
      euclidean_cluster_comparator_->setDistanceThreshold ( 0.01f, false); // 0.01f

      pcl::PointCloud<pcl::Label> euclidean_labels;
      std::vector<pcl::PointIndices> euclidean_label_indices;
      pcl::OrganizedConnectedComponentSegmentation<PointType, pcl::Label> seg_ (euclidean_cluster_comparator_);
      seg_.setInputCloud (cloud_ptr);
      seg_.segment (euclidean_labels, euclidean_label_indices);

      for (size_t i = 0; i < euclidean_label_indices.size (); i++) {
	if (euclidean_label_indices[i].indices.size () > clusterMinSize ) {
	  pcl::PointCloud<PointType> cluster;
	  pcl::copyPointCloud ( *cloud_ptr, euclidean_label_indices[i].indices, cluster);
	  clusters.push_back (cluster);
	}
      }
    }
    FPS_CALC_END("segmentOrganized");
  }

  // ---------------------------------------
  // Cloud callback function (ROS)
  // -----------------------------------------

  void
  cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
  {
    boost::mutex::scoped_lock lock (mtx_);
    static int counter = 0;

    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    CloudPtr cloud (new Cloud);
    pcl::fromROSMsg (*input, *cloud);

    //std::cout << "Cloud size: " << cloud->width << " x " << cloud->height << std::endl;

    double start = pcl::getTime ();
    FPS_CALC_BEGIN;

    cloud_.reset (new Cloud);
    filterPassThrough (cloud, *cloud_);

    if ( counter == 1 ) {

      PCL_INFO ("segmentation, please wait...\n");
      pcl::PointCloud<pcl::Normal>::Ptr normals_ (new pcl::PointCloud<pcl::Normal>);
      integralEstimate (cloud_, normals_);

      pcl::PointCloud<RefPointType>::CloudVectorType clusters;
      segmentOrganized( cloud_, normals_, clusters);
      clusters_ = clusters;
      std::cout << "clusters: " << clusters_.size() << std::endl;

      // Init trackers with segmented cluster clouds
      initTrackers (clusters_.size());
      for (size_t i = 0; i < clusters_.size(); i++) {

	RefCloudPtr ref_cloud (new RefCloud);
	removeZeroPoints (clusters_[i], *ref_cloud);

	Eigen::Vector4f cent;
	pcl::compute3DCentroid<RefPointType> (*ref_cloud, cent);
	Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
	trans.translation ().matrix () = Eigen::Vector3f (cent[0], cent[1], cent[2]);
	pcl::transformPointCloud<RefPointType> (*ref_cloud, *ref_cloud, trans.inverse());

	CloudPtr reduced_cloud (new Cloud);
	gridSample ( ref_cloud, *reduced_cloud, false);
	trackers_[i]->setReferenceCloud (reduced_cloud);
	trackers_[i]->setTrans (trans);
	references_.push_back(ref_cloud);
	trackers_[i]->setMinIndices (int (ref_cloud->size ()) / 2);
      }
      have_clusters_ = true;
    }
    else {
      if (have_clusters_) {
	CloudPtr reduced_cloud (new Cloud);
	gridSample ( cloud_, *reduced_cloud);
	tracking (reduced_cloud);
      }
    }
    counter++;
    new_cloud_ = true;
    double end = pcl::getTime ();
    //computation_time_ = end - start;
    FPS_CALC_END("computation");
    //counter_++;
  }

  void
  spin() {
    while (ros::ok()) {
      ros::spin();
    }
  }

// Private
private:

  // ROS variables
  ros::Subscriber sub_;
  ros::NodeHandle nh_;

  pcl::visualization::CloudViewer viewer_;
  CloudPtr cloud_, cloud_ref_;
  boost::mutex mtx_;
  bool new_cloud_;

  pcl::IntegralImageNormalEstimation<PointType, pcl::Normal> nii_;
  pcl::OrganizedMultiPlaneSegmentation<PointType, pcl::Normal, pcl::Label> mps_;

  // Comparator objects
  pcl::PlaneCoefficientComparator<RefPointType, pcl::Normal>::Ptr plane_comparator_;
  pcl::EuclideanPlaneCoefficientComparator<RefPointType, pcl::Normal>::Ptr euclidean_comparator_;
  pcl::RGBPlaneCoefficientComparator<RefPointType, pcl::Normal>::Ptr rgb_comparator_;
  pcl::EdgeAwarePlaneComparator<RefPointType, pcl::Normal>::Ptr edge_aware_comparator_;
  pcl::EuclideanClusterComparator<RefPointType, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_;

  //Segment Objects
  pcl::PointCloud<RefPointType>::CloudVectorType clusters_;
  size_t clusters_size_;
  bool have_clusters_;

  // Tracker
  std::vector< boost::shared_ptr<ParticleFilterOMPTracker<RefPointType, ParticleT> > > trackers_;
  std::vector<CloudPtr> references_;

  // Variables
  double factor_;
  double size_;
  int planeMinSize_;
  int clusterMinSize_;
  double angularTh_;
  double distanceTh_;

  /*
  pcl::visualization::CloudViewer viewer_;
  pcl::PointCloud<pcl::Normal>::Ptr normals_;
  CloudPtr cloud_pass_;
  CloudPtr cloud_pass_downsampled_;
  CloudPtr plane_cloud_;
  CloudPtr nonplane_cloud_;
  CloudPtr cloud_hull_;
  CloudPtr segmented_cloud_;
  CloudPtr reference_;
  std::vector<pcl::Vertices> hull_vertices_;

  boost::mutex mtx_;
  bool new_cloud_;
  pcl::NormalEstimationOMP<PointType, pcl::Normal> ne_; // to store threadpool
  boost::shared_ptr<ParticleFilter> tracker_;
  int counter_;
  bool use_convex_hull_;
  bool visualize_non_downsample_;
  bool visualize_particles_;
  double tracking_time_;
  double computation_time_;
  double downsampling_time_;
  double downsampling_grid_size_;
  */
};

// Main
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "object_viewer_node");
  ros::NodeHandle nh;

  ObjectViewer<pcl::PointXYZRGBA> node(nh);
  node.spin();
  return 0;
}


#pragma once

#include "node.hpp"
#include "random.hpp"
#include "settings.hpp"
#include "data.hpp"
#include "MeanShift.hpp"
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <vector>
#include <cstdint>
#include <ctime>
#include <queue>

#include <unordered_map>

namespace ISUE {
  namespace RelocForests {
    class Point3D {
    public:
      Point3D(double x, double y, double z) : x(x), y(y), z(z) {};
      double x, y, z;
    };

    struct hashFunc{
      size_t operator()(const Point3D &k) const {
        size_t h1 = std::hash<double>()(k.x);
        size_t h2 = std::hash<double>()(k.y);
        size_t h3 = std::hash<double>()(k.z);
        return (h1 ^ (h2 << 1)) ^ h3;
      }
    };

    struct equalsFunc {
      bool operator()(const Point3D &l, const Point3D &r) const{
        return (l.x == r.x) && (l.y == r.y) && (l.z == r.z);
      }
    };

    typedef std::unordered_map<Point3D, uint32_t, hashFunc, equalsFunc> Point3DMap;

    template <typename D, typename RGB>
    class Tree {
    public:
      Tree()
      {
        root_ = new Node<D, RGB>();
        root_->depth_ = 0;
      };

      ~Tree()
      {
        delete root_;
      };

      void WriteTree(std::ostream& o, Node<D, RGB> *node) const
      {
        if (node == nullptr) {
          o.write("#", sizeof('#'));
          return;
        }
        node->Serialize(o);
        WriteTree(o, node->left_);
        WriteTree(o, node->right_);
      }

      void Serialize(std::ostream& stream) const
      {
        const int majorVersion = 0, minorVersion = 0;

        stream.write(binaryFileHeader_, strlen(binaryFileHeader_));
        stream.write((const char*)(&majorVersion), sizeof(majorVersion));
        stream.write((const char*)(&minorVersion), sizeof(minorVersion));

        //stream.write((const char*)(&settings_->max_tree_depth_), sizeof(settings_->max_tree_depth_));

        WriteTree(stream, root_);
      }

      Node<D, RGB>* ReadTree(std::istream& i)
      {
        int flag = i.peek();
        char val = (char)flag;
        if (val == '#') {
          i.get();
          return nullptr;
        }
        Node<D, RGB> *tmp = new Node<D, RGB>();
        tmp->Deserialize(i);
        tmp->left_ = ReadTree(i);
        tmp->right_ = ReadTree(i);
        return tmp;
      }

      Tree* Deserialize(std::istream& stream)
      {
        settings_ = new Settings();

        std::vector<char> buffer(strlen(binaryFileHeader_) + 1);
        stream.read(&buffer[0], strlen(binaryFileHeader_));
        buffer[buffer.size() - 1] = '\0';
        if (strcmp(&buffer[0], binaryFileHeader_) != 0)
          throw std::runtime_error("Unsupported forest format.");

        const int majorVersion = 0, minorVersion = 0;
        stream.read((char*)(&majorVersion), sizeof(majorVersion));
        stream.read((char*)(&minorVersion), sizeof(minorVersion));

        root_ = ReadTree(stream);

      }

      bool IsValidRecurse(Node<D, RGB> *node, bool prevSplit)
      {

        if (!node && prevSplit)
          return false;

        if (node->is_leaf_)
          return true;

        return IsValidRecurse(node->left_, node->is_split_) && IsValidRecurse(node->right_, node->is_split_);
      }

      bool IsValid()
      {
        return IsValidRecurse(root_, true);
      }

      // learner output
      enum DECISION { LEFT, RIGHT, TRASH };

      //  Evaluates weak learner. Decides whether the point should go left or right.
      //  Returns DECISION enum value.
      DECISION eval_learner(DepthAdaptiveRGB<D, RGB> feature, cv::Mat depth_image, cv::Mat rgb_image, cv::Point2i pos)
      {
        bool valid = true;
        float response = feature.GetResponse(depth_image, rgb_image, pos, *settings_, valid);

        if (!valid) // no depth or out of bounds
          return DECISION::TRASH;

        return (DECISION)(response >= feature.GetThreshold());
      }

      // V(S)
      double variance(std::vector<LabeledPixel> labeled_data)
      {
        if (labeled_data.size() == 0)
          return 0.0;
        double V = (1.0f / (double)labeled_data.size());
        double sum = 0.0f;

        // calculate mean of S
        cv::Point3f tmp;
        for (auto p : labeled_data)
          tmp += p.label_;
        uint32_t size = labeled_data.size();
        cv::Point3f mean(tmp.x / size, tmp.y / size, tmp.z / size);

        for (auto p : labeled_data) {
          cv::Point3f val = (p.label_ - mean);
          sum += val.x * val.x  + val.y * val.y + val.z * val.z;
        }

        return V * sum;
      }

      // Q(S_n, \theta)
      double objective_function(std::vector<LabeledPixel> data, std::vector<LabeledPixel> left, std::vector<LabeledPixel> right)
      {
        double var = variance(data);
        double left_val = ((double)left.size() / (double)data.size()) * variance(left);
        double right_val = ((double)right.size() / (double)data.size()) * variance(right);

        return var - (left_val + right_val);
      }

      Eigen::Vector3d GetLeafMode(std::vector<LabeledPixel> S)
      {
        std::vector<Eigen::Vector3d> data;

        // calc mode for leaf, sub-sample N_SS = 500
        for (uint16_t i = 0; i < (S.size() < 500 ? S.size() : 500); i++) {
          auto p = S.at(i);
          Eigen::Vector3d point{ p.label_.x, p.label_.y, p.label_.z };
          data.push_back(point);
        }

        // cluster
        MeanShift ms = MeanShift(nullptr);
        double kernel_bandwidth = 0.01f; // gaussian
        std::vector<Eigen::Vector3d> cluster = ms.cluster(data, kernel_bandwidth);

        // find mode
        std::vector<Point3D> clustered_points;
        for (auto c : cluster)
          clustered_points.push_back(Point3D(floor(c[0] * 10000) / 10000,
                                             floor(c[1] * 10000) / 10000,
                                             floor(c[2] * 10000) / 10000));

        Point3DMap cluster_map;

        for (auto p : clustered_points)
          cluster_map[p]++;

        std::pair<Point3D, uint32_t> mode(Point3D(0.0, 0.0, 0.0), 0);

        for (auto p : cluster_map)
          if (p.second > mode.second)
            mode = p;

        return Eigen::Vector3d(mode.first.x, mode.first.y, mode.first.z);
      }

      void train_recurse(Node<D, RGB> *node, std::vector<LabeledPixel> S)
      {
        uint16_t height = node->depth_;
        if (S.size() == 1 || ((height == settings_->max_tree_depth_ - 1) && S.size() >= 1)) {

          node->mode_ = GetLeafMode(S);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }

        uint32_t num_candidates = 5,
                feature = 0;
        double minimum_objective = DBL_MAX;

        std::vector<DepthAdaptiveRGB<D, RGB> > candidate_params;
        std::vector<LabeledPixel> left_final, right_final;

        for (uint32_t i = 0; i < num_candidates; ++i) {

          // add candidate
          candidate_params.push_back(DepthAdaptiveRGB<D, RGB>::CreateRandom(random_));

          // partition data with candidate
          std::vector<LabeledPixel> left_data, right_data;

          for (uint32_t j = 0; j < S.size(); ++j) {

            LabeledPixel p = S.at(j);
            DECISION val = eval_learner(candidate_params.at(i), data_->GetDepthImage(p.frame_), data_->GetRGBImage(p.frame_), p.pos_);

            switch (val) {
            case LEFT:
              left_data.push_back(S.at(j));
              break;
            case RIGHT:
              right_data.push_back(S.at(j));
              break;
            case TRASH:
              // do nothing
              break;
            }

          }

          // eval tree training objective function and take best
          // todo: ensure objective function is correct
          double objective = objective_function(S, left_data, right_data);

          if (objective < minimum_objective) {
            feature = i;
            minimum_objective = objective;
            left_final = left_data;
            right_final = right_data;
          }
        }

        // split went only one way
        if (left_final.empty()) {
          node->mode_ = GetLeafMode(right_final);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }
        if (right_final.empty()) {
          node->mode_ = GetLeafMode(left_final);
          node->is_leaf_ = true;
          node->left_ = nullptr;
          node->right_ = nullptr;
          return;
        }

        // set feature
        node->is_split_ = true;
        node->is_leaf_ = false;
        node->feature_ = candidate_params.at(feature);
        node->left_ = new Node<D, RGB>();
        node->right_ = new Node<D, RGB>();
        node->left_->depth_ = node->right_->depth_ = node->depth_ + 1;

        train_recurse(node->left_, left_final);
        train_recurse(node->right_, right_final);
      }

      void Train(Data *data, std::vector<LabeledPixel> labeled_data, Random *random, Settings *settings)
      {
        data_ = data;
        random_ = random;
        settings_ = settings;
        train_recurse(root_, labeled_data);
      }

      Eigen::Vector3d eval_recursive(Node<D, RGB> **node, int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid)
      {
        if ((*node)->is_leaf_) {
          return (*node)->mode_;
        }

        DECISION val = eval_learner((*node)->feature_, depth_image, rgb_image, cv::Point2i(col, row));

        switch (val) {
        case LEFT:
          return eval_recursive(&(*node)->left_, row, col, rgb_image, depth_image, valid);
          break;
        case RIGHT:
          return eval_recursive(&(*node)->right_, row, col, rgb_image, depth_image, valid);
          break;
        case TRASH:
          valid = false;
          break;
        }
      }

      // Evaluate tree at a pixel
      Eigen::Vector3d Eval(int row, int col, cv::Mat rgb_image, cv::Mat depth_image, bool &valid)
      {
        auto m = eval_recursive(&root_, row, col, rgb_image, depth_image, valid);
        return m;
      }

    private:
      Node<D, RGB> *root_;
      Data *data_;
      Random *random_;
      Settings *settings_;
      const char* binaryFileHeader_ = "ISUE.RelocForests.Tree";
    };
  }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Jose Aparicio
 Copyright (C) 2008 Chris Kenyon
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2008 StatPro Italia srl
 Copyright (C) 2009 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file hazardratestructure.hpp
    \brief hazard-rate term structure
*/

#ifndef quantlib_hazard_rate_structure_hpp
#define quantlib_hazard_rate_structure_hpp

#include <ql/termstructures/defaulttermstructure.hpp>

namespace QuantLib {

    //! Hazard-rate term structure
    /*! This abstract class acts as an adapter to
        DefaultProbabilityTermStructure allowing the programmer to implement
        only the <tt>hazardRateImpl(Time)</tt> method in derived classes.

        Survival/default probabilities and default densities are calculated
        from hazard rates.

        Hazard rates are defined with annual frequency and continuous
        compounding.

        \ingroup defaultprobabilitytermstructures
    */
    class HazardRateStructure : public DefaultProbabilityTermStructure {
      public:
        /*! \name Constructors
            See the TermStructure documentation for issues regarding
            constructors.
        */
        //@{
        HazardRateStructure(
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        HazardRateStructure(
            const Date& referenceDate,
            const Calendar& cal = Calendar(),
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        HazardRateStructure(
            Natural settlementDays,
            const Calendar& cal,
            const DayCounter& dayCounter = DayCounter(),
            const std::vector<Handle<Quote> >& jumps = std::vector<Handle<Quote> >(),
            const std::vector<Date>& jumpDates = std::vector<Date>());
        //@}
      protected:
        /*! \name Calculations

            This method must be implemented in derived classes to
            perform the actual calculations. When it is called,
            range check has already been performed; therefore, it
            must assume that extrapolation is required.
        */
        //@{
        //! hazard rate calculation
        virtual Real hazardRateImpl(Time) const = 0;
        //@}

        //! \name DefaultProbabilityTermStructure implementation
        //@{
        /*! survival probability calculation
            implemented in terms of the hazard rate \f$ h(t) \f$ as
            \f[
            S(t) = \exp\left( - \int_0^t h(\tau) d\tau \right).
            \f]

            \warning This default implementation uses numerical integration,
                     which might be inefficient and inaccurate.
                     Derived classes should override it if a more efficient
                     implementation is available.
        */
        Probability survivalProbabilityImpl(Time) const;
        //! default density calculation
        Real defaultDensityImpl(Time) const;
        //@}
    };

    // inline definitions

    inline Real HazardRateStructure::defaultDensityImpl(Time t) const {
        return hazardRateImpl(t)*survivalProbabilityImpl(t);
    }

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Jose Aparicio
 Copyright (C) 2008 Chris Kenyon
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2008 StatPro Italia srl
 Copyright (C) 2009 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/integrals/gaussianquadratures.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

using namespace boost;

namespace QuantLib {

    namespace {

        template <class F>
        struct remapper {
            F f;
            Time T;
            remapper(const F& f, Time T) : f(f), T(T) {}
            // This remaps [-1,1] to [0,T]. No differential included.
            Real operator()(Real x) const {
                const Real arg = (x+1.0)*T/2.0;
                return f(arg);
            }
        };

        template <class F>
        remapper<F> remap(const F& f, Time T) {
            return remapper<F>(f,T);
        }

    }

    inline HazardRateStructure::HazardRateStructure(
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(dc, jumps, jumpDates) {}

    inline HazardRateStructure::HazardRateStructure(
                                    const Date& refDate,
                                    const Calendar& cal,
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(refDate, cal, dc, jumps, jumpDates) {}

    inline HazardRateStructure::HazardRateStructure(
                                    Natural settlDays,
                                    const Calendar& cal,
                                    const DayCounter& dc,
                                    const std::vector<Handle<Quote> >& jumps,
                                    const std::vector<Date>& jumpDates)
    : DefaultProbabilityTermStructure(settlDays, cal, dc, jumps, jumpDates) {}

    inline Probability HazardRateStructure::survivalProbabilityImpl(Time t) const {
        static GaussChebyshevIntegration integral(48);
        // this stores the address of the method to integrate (so that
        // we don't have to insert its full expression inside the
        // integral below--it's long enough already)
        Real (HazardRateStructure::*f)(Time) const =
            &HazardRateStructure::hazardRateImpl;
        // the Gauss-Chebyshev quadratures integrate over [-1,1],
        // hence the remapping (and the Jacobian term t/2)
        return std::exp(-integral(remap(bind(f,this,_1), t)) * t/2.0);
    }

}

#endif


#include "IterativeDeepeningAgent.hpp"
#include <sstream>
#include <boost/throw_exception.hpp>
#include "core/Action.hpp"
#include "util/geometry/geometry.hpp"
#include "util/define_logger.hpp"
#include "core/Exception.hpp"
#include "core/registerAgent.hpp"

using namespace aima::path_finder;
using aima::util::geometry::Point;

AIMA_REGISTER_AGENT( PathFinderAgent, IterativeDeepeningAgent ); // NOLINT(cert-err58-cpp)

DEFINE_LOGGER( IterativeDeepeningAgent )

std::unique_ptr<aima::core::Agent> IterativeDeepeningAgent::clone() const {
    return std::unique_ptr<aima::core::Agent>( new IterativeDeepeningAgent{ *this } );
}

PathFinderAgent::Generator IterativeDeepeningAgent::search( Point currentLocation,
                                                            Point goal,
                                                            const PathFinderEnvironment::Obstacles& obstacles ) {
    return iterativeDeepeningSearch(
            makeNode<SearchNode>( {}, currentLocation, 0 ),
            goal,
            obstacles );
}

IterativeDeepeningAgent::Generator
IterativeDeepeningAgent::iterativeDeepeningSearch( std::shared_ptr<SearchNode> node,
                                                   Point goal,
                                                   const PathFinderEnvironment::Obstacles& obstacles ) {
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
    for ( size_t depth = 1; true; ++depth ) {
        LOG4CPLUS_DEBUG( GetLogger(), "Iterative deepening search depth " << depth );
        auto       search = depthLimitedSearch( node, goal, obstacles, depth );
        for ( auto response: search ) {
            switch ( response ) {
                case SearchResults::FAIL: co_yield SearchResults::BUSY;
                    break;
                case SearchResults::SUCCESS: isAlive( false );
                    [[fallthrough]];
                case SearchResults::BUSY: co_yield response;
            }
        }
    }
#pragma clang diagnostic pop
}

IterativeDeepeningAgent::Generator
IterativeDeepeningAgent::depthLimitedSearch( const std::shared_ptr<SearchNode>& node,
                                             Point goal,
                                             const PathFinderEnvironment::Obstacles& obstacles,
                                             size_t limit ) {
    if ( !node )
        BOOST_THROW_EXCEPTION( std::logic_error{ "depth-limited search started with a null search node" } );

    LOG4CPLUS_DEBUG( GetLogger(), "Checking node at " << node->location << " with limit " << limit );
    LOG4CPLUS_DEBUG( GetLogger(), "Depth limited search - Checking if reached goal " << goal );
    if ( node->location == goal ) {
        LOG4CPLUS_DEBUG( GetLogger(), "Reached goal " << goal );
        plan = node;
        co_yield SearchResults::SUCCESS;
        co_return;
    }
    LOG4CPLUS_DEBUG( GetLogger(), "Depth limited search - Checking if reached limit" );
    if ( limit == 0 ) {
        co_yield SearchResults::FAIL;
        co_return;
    }

    const auto         reachablePoints = triviallyReachablePoints( node->location, obstacles, goal );
    std::ostringstream ss;
    ss << reachablePoints.size() << " reachable points: ";
    std::copy( reachablePoints.begin(), reachablePoints.end(), std::ostream_iterator<Point>{ ss, "," } );
    LOG4CPLUS_DEBUG( GetLogger(), ss.str());

    co_yield SearchResults::BUSY;

    for ( const auto& point: reachablePoints ) {
        auto& child = node->addChild(
                makeNode<SearchNode>( node, point, node->pathCost + distance( node->location, point )));
        plan = child;

        co_yield depthLimitedSearch( child, goal, obstacles, limit - 1 );
        node->children = {};
    }

    plan = node;
    co_yield SearchResults::FAIL;
}


/// \file
/// Maintainer: Luzian Hug
///
///

#include "mean_shift.h"

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <random>
#include <set>
#include <stdlib.h>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE(mean_shift_read_write) {
  MouseTrack::PointCloud pc;
  MouseTrack::MeanShift ms = MouseTrack::MeanShift(1);
  ms(pc);
}

BOOST_AUTO_TEST_CASE(mean_shift_single_point) {
  MouseTrack::MeanShift ms = MouseTrack::MeanShift(1);

  MouseTrack::PointCloud pc;
  pc.resize(1, 0);
  pc[0].x(0);
  pc[0].y(3);
  pc[0].z(5);
  pc[0].intensity(1);

  std::vector<MouseTrack::Cluster> clusters = ms(pc);
  BOOST_CHECK_EQUAL(clusters.size(), 1);
}

BOOST_AUTO_TEST_CASE(two_faraway_points) {
  MouseTrack::MeanShift ms = MouseTrack::MeanShift(1);
  MouseTrack::PointCloud pc;
  pc.resize(2, 0);
  pc[0].x(0);
  pc[0].y(0);
  pc[0].z(0);
  pc[0].intensity(0);
  pc[1].x(100);
  pc[1].y(100);
  pc[1].z(100);
  pc[1].intensity(1);

  std::vector<MouseTrack::Cluster> clusters = ms(pc);
  BOOST_CHECK_EQUAL(clusters.size(), 2);
}

BOOST_AUTO_TEST_CASE(two_closetogether_points) {
  MouseTrack::MeanShift ms = MouseTrack::MeanShift(1);
  MouseTrack::PointCloud pc;
  pc.resize(2, 0);
  pc[0].x(0);
  pc[0].y(0);
  pc[0].z(0);
  pc[0].intensity(0);
  pc[1].x(0.1);
  pc[1].y(0.1);
  pc[1].z(0.1);
  pc[1].intensity(0);

  std::vector<MouseTrack::Cluster> clusters = ms(pc);
  BOOST_CHECK_EQUAL(clusters.size(), 1);
}

BOOST_AUTO_TEST_CASE(three_gaussian_clusters) {
  std::default_random_engine gen;

  std::normal_distribution<double> gauss0(0.0, 1.0);
  std::normal_distribution<double> gauss10(100.0, 1.0);

  MouseTrack::PointCloud pc;
  pc.resize(300, 0);

  MouseTrack::MeanShift ms = MouseTrack::MeanShift(2.0);

  for (int i = 0; i < 300; i += 3) {
    pc[i].x(gauss10(gen));
    pc[i].y(gauss0(gen));
    pc[i].z(gauss0(gen));
    pc[i].intensity(gauss0(gen));

    pc[i + 1].x(gauss0(gen));
    pc[i + 1].y(gauss10(gen));
    pc[i + 1].z(gauss0(gen));
    pc[i + 1].intensity(gauss0(gen));

    pc[i + 2].x(gauss0(gen));
    pc[i + 2].y(gauss0(gen));
    pc[i + 2].z(gauss10(gen));
    pc[i + 2].intensity(gauss0(gen));
  }

  std::vector<MouseTrack::Cluster> clusters = ms(pc);

  BOOST_CHECK_EQUAL(clusters.size(), 3);
  BOOST_CHECK_EQUAL(clusters[0].points().size(), 100);
  BOOST_CHECK_EQUAL(clusters[1].points().size(), 100);
  BOOST_CHECK_EQUAL(clusters[2].points().size(), 100);
}

BOOST_AUTO_TEST_CASE(expected_clustering) {
  MouseTrack::PointCloud pc;
  pc.resize(10, 0);
  for (int i = 0; i < 10; i += 1) {
    pc[i].y(0);
    pc[i].z(0);
    pc[i].intensity(0);
  }
  pc[0].x(1.0);
  pc[1].x(2.0);
  pc[2].x(2.0);
  pc[3].x(3.0);
  pc[4].x(10.0);
  pc[5].x(11.0);
  pc[6].x(12.0);
  pc[7].x(100.0);
  pc[8].x(101.0);
  pc[9].x(102.0);

  std::vector<std::multiset<size_t>> expected{3};
  // first cluster
  expected[0].insert(0);
  expected[0].insert(1);
  expected[0].insert(2);
  expected[0].insert(3);
  // second cluster
  expected[1].insert(4);
  expected[1].insert(5);
  expected[1].insert(6);
  // third cluster
  expected[2].insert(7);
  expected[2].insert(8);
  expected[2].insert(9);

  MouseTrack::MeanShift ms = MouseTrack::MeanShift(1.0);

  std::vector<MouseTrack::Cluster> clusters = ms(pc);

  BOOST_CHECK_EQUAL(clusters.size(), 3);

  std::vector<std::multiset<size_t>> received{3};
  for (size_t i = 0; i < received.size(); ++i) {
    received[i].insert(clusters[i].points().begin(),
                       clusters[i].points().end());
  }

  BOOST_CHECK(expected[0] == received[0]);
  BOOST_CHECK(expected[1] == received[1]);
  BOOST_CHECK(expected[2] == received[2]);
}


#ifndef __UASTAR_UTIL
#define __UASTAR_UTIL

#include <algorithm>
#include <cassert>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <exception>
#include <iostream>
#include <limits>
#include <memory>
#include <stdexcept>
#include <stdint.h>
#include <string>
#include <vector>

// workaround to fix bug https://svn.boost.org/trac/boost/ticket/9392
#if defined(__CUDACC__)
#define BOOST_NOINLINE __attribute__ ((noinline))
#endif

#include <boost/program_options/variables_map.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>

// C++11 is not well supported by CUDA 5.5
#ifndef NO_CPP11
#include <random>
#endif

using std::cin;
using std::cout;
using std::flush;
using std::min;
using std::max;
using std::endl;
using std::pair;
using std::runtime_error;
using std::string;
using std::vector;
using std::make_pair;
using std::numeric_limits;

using boost::lexical_cast;

#ifndef NO_CPP11
extern boost::mt19937 random_engine;
#endif
extern boost::program_options::variables_map vm_options;
extern void help();
extern bool debug;

#ifndef DEBUG
#  define DEBUG 0
#endif
#define DEBUG_CONDITION (DEBUG && debug)
#define dprintf(fmt, ...) \
    do { \
        if (DEBUG_CONDITION) { \
            fprintf(stderr, fmt, __VA_ARGS__); \
            fflush(stderr); \
        } \
    } while (0)

#define dout if (!DEBUG_CONDITION) {} else std::cerr

union Float_t
{
    Float_t(float num = 0.0f) : f(num) {}
    // Portable extraction of components.
    bool Negative() const { return (i >> 31) != 0; }
    int32_t RawMantissa() const { return i & ((1 << 23) - 1); }
    int32_t RawExponent() const { return (i >> 23) & 0xFF; }

    int32_t i;
    float f;
    struct
    {   // Bitfields for exploration. Do not use in production code.
        uint32_t mantissa : 23;
        uint32_t exponent : 8;
        uint32_t sign : 1;
    } parts;
};

inline bool float_equal(
    float A,
    float B,
    float maxDiff = 1e-3f,
    int maxUlpsDiff = 200)
{
    // Check if the numbers are really close -- needed
    // when comparing numbers near zero.
    float absDiff = fabs(A - B);
    if (absDiff <= maxDiff)
        return true;

    Float_t uA(A);
    Float_t uB(B);

    // Different signs means they do not match.
    if (uA.Negative() != uB.Negative())
        return false;

    // Find the difference in ULPs.
    int ulpsDiff = abs(uA.i - uB.i);
    if (ulpsDiff <= maxUlpsDiff)
        return true;

    return false;
}

#ifdef __CUDACC__
#  define __cuda__ __host__ __device__
#else
#  define __cuda__
#endif

const float SQRT2 = 1.4142135623731f;
const int DX[8] = { 1, -1,  0,  0,  1,  1, -1, -1, };
const int DY[8] = { 0,  0,  1, -1,  1, -1,  1, -1, };
const float COST[8] = { 1, 1, 1, 1, SQRT2, SQRT2, SQRT2, SQRT2, };

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/greedy/car_fueling.hpp"

BOOST_AUTO_TEST_SUITE(TestCarFueling)

BOOST_AUTO_TEST_CASE(test)
{
    {
        const uint32_t city_dist = 0;
        const uint32_t tank_dist = 100;
        std::vector<uint32_t> stops = { 50 };
        const int expected_refills = 0;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 10;
        const uint32_t tank_dist = 0;
        std::vector<uint32_t> stops = { 50 };
        const int expected_refills = -1;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 10;
        const uint32_t tank_dist = 20;
        std::vector<uint32_t> stops = { };
        const int expected_refills = 0;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 950;
        const uint32_t tank_dist = 400;
        std::vector<uint32_t> stops = { 200, 375, 550, 750 };
        const int expected_refills = 2;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 10;
        const uint32_t tank_dist = 3;
        std::vector<uint32_t> stops = { 1, 2, 5, 9 };
        const int expected_refills = -1;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 10;
        const uint32_t tank_dist = 3;
        std::vector<uint32_t> stops = { 4, 5, 9 };
        const int expected_refills = -1;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }

    {
        const uint32_t city_dist = 15;
        const uint32_t tank_dist = 3;
        std::vector<uint32_t> stops = { 2, 5, 8 };
        const int expected_refills = -1;
        const auto refills =
                Algo::Greedy::CarFueling::MinRefills(city_dist, tank_dist, stops);
        BOOST_CHECK(refills == expected_refills);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/***********************************************************************
Copyright (c) 2017, Shushman Choudhury
All rights reserved.
Authors: Shushman Choudhury <shushmanchoudhury@gmail.com>

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.
*************************************************************************/
#ifndef BATCHING_POMP_HYBRID_BATCHING_HPP_
#define BATCHING_POMP_HYBRID_BATCHING_HPP_

#include <exception>
#include <ompl/base/StateSpace.h>
#include <ompl/util/Console.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include "batching_pomp/batching/BatchingManager.hpp"

namespace batching_pomp {
namespace batching {

//! Derived class of BatchingManager that implements Hybrid Batching.

/// Implements Hybrid Batching, which proceeds in two phases.
/// Initially, batches of vertices are added, and the r-disk
/// shrinks (as per a function connecting vertices to radius)
/// to maintain a specific relationship between edges and vertices.
/// Once all vertices are added, batches of edges are added based on
/// an increasing radius of connectivity (similar to Edge Batching)
template<class Graph, class VStateMap, class StateCon, class EDistance>
class HybridBatching : public BatchingManager<Graph, VStateMap, StateCon, EDistance>
{

typedef boost::graph_traits<Graph> GraphTypes;
typedef typename GraphTypes::vertex_iterator VertexIter;
typedef typename GraphTypes::vertex_descriptor Vertex;

public:

  /// \param[in] _initNumVertices The initial number of vertices to begin with
  /// \param[in] _vertInflFactor The factor by which to increase the number of
  ///                            vertices in each batch in the first phase
  /// \param[in] _radiusInflFactor The value by which to increase
  ///                             the radius for each new batch in the second phase
  /// \param[in] _initRadiusFn The function which generates the radius based on
  ///                          the number of vertices
  /// \param[in] _maxRadius The maximum meaningful radius for edge batching.
  ///                        It is a function of the space bounds and the current solution quality.
  HybridBatching(const ompl::base::StateSpacePtr _space,
                 VStateMap _stateMap,
                 std::string _roadmapFileName,
                 Graph& _fullRoadmap,
                 Graph& _currentRoadmap,
                 unsigned int _initNumVertices,
                 double _vertInflFactor,
                 double _radiusInflFactor,
                 const std::function<double(unsigned int)>& _initRadiusFn,
                 double _maxRadius
                 )
  : BatchingManager<Graph, VStateMap, StateCon, EDistance>(_space,_stateMap,_roadmapFileName,_fullRoadmap,_currentRoadmap)
  , mNumVerticesAdded{0u}
  , mNextVertexTarget{_initNumVertices}
  , mVertInflFactor{_vertInflFactor}
  , mRadiusInflFactor{_radiusInflFactor}
  , mInitRadius{_initRadiusFn(_initNumVertices)}
  , mMaxRadius{_maxRadius}
  , mEdgeBatchingMode{false}
  , mRadiusFn{_initRadiusFn}
  {
    BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius = mInitRadius;
    boost::tie(mCurrVertex,mLastVertex) = vertices(BatchingManager<Graph, VStateMap, StateCon, EDistance>::mFullRoadmap);
  }

  //////////////////////////////////////////////////
  /// Setters and Getters
  void setVertexInflationFactor(double _vertInflFactor)
  {
    mVertInflFactor = _vertInflFactor;
  }

  double getVertexInflationFactor() const
  {
    return mVertInflFactor;
  }

  void setRadiusInflationFactor(unsigned int _radiusInflFactor)
  {
    mRadiusInflFactor = _radiusInflFactor;
  }

  double getRadiusInflationFactor() const
  {
    return mRadiusInflFactor;
  }

  void setInitRadius(double _initRadius)
  {
    mInitRadius = _initRadius;
  }

  double getInitRadius() const
  {
    return mInitRadius;
  }

  void setMaxRadius(double _maxRadius)
  {
    mMaxRadius = _maxRadius;
  }

  double getMaxRadius() const
  {
    return mMaxRadius;
  }

  bool isInEdgeBatchingMode() const
  {
    return mEdgeBatchingMode;
  }

  //////////////////////////////////////////////////
  /// Overriden methods
  void updateWithNewSolutionCost(double _newSolnCost) override
  {
    mMaxRadius = std::min(mMaxRadius,_newSolnCost);
  }

  void nextBatch(const std::function<bool(const ompl::base::State*)>& _pruneFunction,
                 ompl::NearestNeighbors<Vertex>& _vertexNN) override
  {

    if(BatchingManager<Graph, VStateMap, StateCon, EDistance>::mExhausted){
      OMPL_INFORM("Batching exhausted! No updates with nextBatch!");
      return;
    }

    OMPL_INFORM("New Hybrid Batch called!");
    ++BatchingManager<Graph, VStateMap, StateCon, EDistance>::mNumBatches;

    if(!mEdgeBatchingMode)
    {
      std::vector<Vertex> vertex_vector(mNextVertexTarget - mNumVerticesAdded);
      size_t idx{0};

      while(mNumVerticesAdded < mNextVertexTarget)
      {

        if(!_pruneFunction(BatchingManager<Graph, VStateMap, StateCon, EDistance>::mFullRoadmap[*mCurrVertex].v_state->state)) {
          Vertex newVertex{boost::add_vertex(BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrentRoadmap)};
          BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrentRoadmap[newVertex].v_state = BatchingManager<Graph, VStateMap, StateCon, EDistance>::mFullRoadmap[*mCurrVertex].v_state;
          vertex_vector[idx++] = newVertex;
        }

        ++mCurrVertex;
        ++mNumVerticesAdded;

        // If all samples added, next round will be edge batching mode
        if(mCurrVertex == mLastVertex) {
          mEdgeBatchingMode = true;
          break;
        }
      }

      if(idx > 0u) {
        vertex_vector.resize(idx);
        _vertexNN.add(vertex_vector);
      }

      BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius = mRadiusFn(std::min(mNextVertexTarget,BatchingManager<Graph, VStateMap, StateCon, EDistance>::mNumVertices));

      mNextVertexTarget = static_cast<unsigned int>(mNextVertexTarget * mVertInflFactor);
    }
    else
    {
      BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius = BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius * mRadiusInflFactor;

      if(BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius > mMaxRadius)
      {
        BatchingManager<Graph, VStateMap, StateCon, EDistance>::mCurrRadius = mMaxRadius;
        BatchingManager<Graph, VStateMap, StateCon, EDistance>::mExhausted = true;
      }
    }

  }

private:

  unsigned int mNumVerticesAdded;
  unsigned int mNextVertexTarget;
  double mVertInflFactor;

  double mRadiusInflFactor;
  double mInitRadius;
  double mMaxRadius;

  VertexIter mCurrVertex;
  VertexIter mLastVertex;

  bool mEdgeBatchingMode;
  std::function<double(unsigned int)> mRadiusFn;

};

} // namespace batching_pomp
} // namespace batching
#endif //BATCHING_POMP_HYBRID_BATCHING_HPP_