// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_CMATH_HPP
#define BOOST_UNITS_CMATH_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <cstdlib>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/hypot.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/sign.hpp>

#include <boost/units/dimensionless_quantity.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/detail/cmath_impl.hpp>
#include <boost/units/detail/dimensionless_unit.hpp>

#include <boost/units/systems/si/plane_angle.hpp>

/// \file
/// \brief Overloads of functions in \<cmath\> for quantities.
/// \details Only functions for which a dimensionally-correct result type
///   can be determined are overloaded.
///   All functions work with dimensionless quantities.

// BOOST_PREVENT_MACRO_SUBSTITUTION is needed on certain compilers that define
// some <cmath> functions as macros; it is used for all functions even though it
// isn't necessary -- I didn't want to think :)
//
// the form using namespace detail; return(f(x)); is used
// to enable ADL for UDTs.

namespace boost {

namespace units {

template<class Unit,class Y>
inline
bool
isfinite BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::isfinite;
    return isfinite BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
bool
isinf BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::isinf;
    return isinf BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
bool
isnan BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::isnan;
    return isnan BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
bool
isnormal BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::isnormal;
    return isnormal BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
bool
isgreater BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                            const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return isgreater BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
bool
isgreaterequal BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                                 const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return isgreaterequal BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
bool
isless BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                         const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return isless BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
bool
islessequal BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                              const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return islessequal BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
bool
islessgreater BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                                const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return islessgreater BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
bool
isunordered BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                              const quantity<Unit,Y>& q2)
{
    using namespace detail;
    return isunordered BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value());
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
abs BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using std::abs;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(abs BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
ceil BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using std::ceil;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(ceil BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
copysign BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
         const quantity<Unit,Y>& q2)
{
    using boost::math::copysign;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(copysign BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
fabs BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using std::fabs;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(fabs BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
floor BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using std::floor;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(floor BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
fdim BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                       const quantity<Unit,Y>& q2)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(fdim BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

#if 0

template<class Unit1,class Unit2,class Unit3,class Y>
inline
typename add_typeof_helper<
    typename multiply_typeof_helper<quantity<Unit1,Y>,
                                    quantity<Unit2,Y> >::type,
    quantity<Unit3,Y> >::type
fma BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit1,Y>& q1,
                                      const quantity<Unit2,Y>& q2,
                                      const quantity<Unit3,Y>& q3)
{
    using namespace detail;

    typedef quantity<Unit1,Y>   type1;
    typedef quantity<Unit2,Y>   type2;
    typedef quantity<Unit3,Y>   type3;

    typedef typename multiply_typeof_helper<type1,type2>::type  prod_type;
    typedef typename add_typeof_helper<prod_type,type3>::type   quantity_type;

    return quantity_type::from_value(fma BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value(),q3.value()));
}

#endif

template<class Unit,class Y>
inline
quantity<Unit,Y>
fmax BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                       const quantity<Unit,Y>& q2)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(fmax BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
fmin BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                       const quantity<Unit,Y>& q2)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(fmin BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

template<class Unit,class Y>
inline
int
fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::fpclassify;

    return fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
typename root_typeof_helper<
    typename add_typeof_helper<
        typename power_typeof_helper<quantity<Unit,Y>,
                                     static_rational<2> >::type,
        typename power_typeof_helper<quantity<Unit,Y>,
                                     static_rational<2> >::type>::type,
        static_rational<2> >::type
hypot BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,const quantity<Unit,Y>& q2)
{
    using boost::math::hypot;

    typedef quantity<Unit,Y>    type1;

    typedef typename power_typeof_helper<type1,static_rational<2> >::type   pow_type;
    typedef typename add_typeof_helper<pow_type,pow_type>::type             add_type;
    typedef typename root_typeof_helper<add_type,static_rational<2> >::type quantity_type;

    return quantity_type::from_value(hypot BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

// does ISO C++ support long long? g++ claims not
//template<class Unit,class Y>
//inline
//quantity<Unit,long long>
//llrint BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
//{
//    using namespace detail;
//
//    typedef quantity<Unit,long long>    quantity_type;
//
//    return quantity_type::from_value(llrint BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
//}

// does ISO C++ support long long? g++ claims not
//template<class Unit,class Y>
//inline
//quantity<Unit,long long>
//llround BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
//{
//    using namespace detail;
//
//    typedef quantity<Unit,long long>    quantity_type;
//
//    return quantity_type::from_value(llround BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
//}

#if 0

template<class Unit,class Y>
inline
quantity<Unit,Y>
nearbyint BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(nearbyint BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

#endif

template<class Unit,class Y>
inline
quantity<Unit,Y> nextafter BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                                             const quantity<Unit,Y>& q2)
{
    using boost::math::nextafter;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(nextafter BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}
template<class Unit,class Y>
inline
quantity<Unit,Y> nexttoward BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q1,
                                                              const quantity<Unit,Y>& q2)
{
    // the only difference between nextafter and nexttowards is
    // in the argument types.  Since we are requiring identical
    // argument types, there is no difference.
    using boost::math::nextafter;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(nextafter BOOST_PREVENT_MACRO_SUBSTITUTION (q1.value(),q2.value()));
}

#if 0

template<class Unit,class Y>
inline
quantity<Unit,Y>
rint BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(rint BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

#endif

template<class Unit,class Y>
inline
quantity<Unit,Y>
round BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::round;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(round BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
int
signbit BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using boost::math::signbit;

    return signbit BOOST_PREVENT_MACRO_SUBSTITUTION (q.value());
}

template<class Unit,class Y>
inline
quantity<Unit,Y>
trunc BOOST_PREVENT_MACRO_SUBSTITUTION (const quantity<Unit,Y>& q)
{
    using namespace detail;

    typedef quantity<Unit,Y>    quantity_type;

    return quantity_type::from_value(trunc BOOST_PREVENT_MACRO_SUBSTITUTION (q.value()));
}

template<class Unit,class Y>
inline
quantity<Unit, Y>
fmod(const quantity<Unit,Y>& q1, const quantity<Unit,Y>& q2)
{
    using std::fmod;

    typedef quantity<Unit,Y> quantity_type;

    return quantity_type::from_value(fmod(q1.value(), q2.value()));
}

template<class Unit, class Y>
inline
quantity<Unit, Y>
modf(const quantity<Unit, Y>& q1, quantity<Unit, Y>* q2)
{
    using std::modf;

    typedef quantity<Unit,Y> quantity_type;

    return quantity_type::from_value(modf(q1.value(), &quantity_cast<Y&>(*q2)));
}

template<class Unit, class Y, class Int>
inline
quantity<Unit, Y>
frexp(const quantity<Unit, Y>& q,Int* ex)
{
    using std::frexp;

    typedef quantity<Unit,Y> quantity_type;

    return quantity_type::from_value(frexp(q.value(),ex));
}

/// For non-dimensionless quantities, integral and rational powers
/// and roots can be computed by @c pow<Ex> and @c root<Rt> respectively.
template<class S, class Y>
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
pow(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q1,
    const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q2)
{
    using std::pow;

    typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S),Y> quantity_type;

    return quantity_type::from_value(pow(q1.value(), q2.value()));
}

template<class S, class Y>
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
exp(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q)
{
    using std::exp;

    typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y> quantity_type;

    return quantity_type::from_value(exp(q.value()));
}

template<class Unit, class Y, class Int>
inline
quantity<Unit, Y>
ldexp(const quantity<Unit, Y>& q,const Int& ex)
{
    using std::ldexp;

    typedef quantity<Unit,Y> quantity_type;

    return quantity_type::from_value(ldexp(q.value(), ex));
}

template<class S, class Y>
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
log(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q)
{
    using std::log;

    typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y> quantity_type;

    return quantity_type::from_value(log(q.value()));
}

template<class S, class Y>
inline
quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>
log10(const quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y>& q)
{
    using std::log10;

    typedef quantity<BOOST_UNITS_DIMENSIONLESS_UNIT(S), Y> quantity_type;

    return quantity_type::from_value(log10(q.value()));
}

template<class Unit,class Y>
inline
typename root_typeof_helper<
                            quantity<Unit,Y>,
                            static_rational<2>
                           >::type
sqrt(const quantity<Unit,Y>& q)
{
    using std::sqrt;

    typedef typename root_typeof_helper<
                                        quantity<Unit,Y>,
                                        static_rational<2>
                                       >::type quantity_type;

    return quantity_type::from_value(sqrt(q.value()));
}

} // namespace units

} // namespace boost

namespace boost {

namespace units {

// trig functions with si argument/return types

/// cos of theta in radians
template<class Y>
typename dimensionless_quantity<si::system,Y>::type
cos(const quantity<si::plane_angle,Y>& theta)
{
    using std::cos;
    return cos(theta.value());
}

/// sin of theta in radians
template<class Y>
typename dimensionless_quantity<si::system,Y>::type
sin(const quantity<si::plane_angle,Y>& theta)
{
    using std::sin;
    return sin(theta.value());
}

/// tan of theta in radians
template<class Y>
typename dimensionless_quantity<si::system,Y>::type
tan(const quantity<si::plane_angle,Y>& theta)
{
    using std::tan;
    return tan(theta.value());
}

/// cos of theta in other angular units
template<class System,class Y>
typename dimensionless_quantity<System,Y>::type
cos(const quantity<unit<plane_angle_dimension,System>,Y>& theta)
{
    return cos(quantity<si::plane_angle,Y>(theta));
}

/// sin of theta in other angular units
template<class System,class Y>
typename dimensionless_quantity<System,Y>::type
sin(const quantity<unit<plane_angle_dimension,System>,Y>& theta)
{
    return sin(quantity<si::plane_angle,Y>(theta));
}

/// tan of theta in other angular units
template<class System,class Y>
typename dimensionless_quantity<System,Y>::type
tan(const quantity<unit<plane_angle_dimension,System>,Y>& theta)
{
    return tan(quantity<si::plane_angle,Y>(theta));
}

/// acos of dimensionless quantity returning angle in same system
template<class Y,class System>
quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>
acos(const quantity<unit<dimensionless_type, homogeneous_system<System> >,Y>& val)
{
    using std::acos;
    return quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>(acos(val.value())*si::radians);
}

/// acos of dimensionless quantity returning angle in radians
template<class Y>
quantity<angle::radian_base_unit::unit_type,Y>
acos(const quantity<unit<dimensionless_type, heterogeneous_dimensionless_system>,Y>& val)
{
    using std::acos;
    return quantity<angle::radian_base_unit::unit_type,Y>::from_value(acos(val.value()));
}

/// asin of dimensionless quantity returning angle in same system
template<class Y,class System>
quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>
asin(const quantity<unit<dimensionless_type, homogeneous_system<System> >,Y>& val)
{
    using std::asin;
    return quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>(asin(val.value())*si::radians);
}

/// asin of dimensionless quantity returning angle in radians
template<class Y>
quantity<angle::radian_base_unit::unit_type,Y>
asin(const quantity<unit<dimensionless_type, heterogeneous_dimensionless_system>,Y>& val)
{
    using std::asin;
    return quantity<angle::radian_base_unit::unit_type,Y>::from_value(asin(val.value()));
}

/// atan of dimensionless quantity returning angle in same system
template<class Y,class System>
quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>
atan(const quantity<unit<dimensionless_type, homogeneous_system<System> >, Y>& val)
{
    using std::atan;
    return quantity<unit<plane_angle_dimension, homogeneous_system<System> >,Y>(atan(val.value())*si::radians);
}

/// atan of dimensionless quantity returning angle in radians
template<class Y>
quantity<angle::radian_base_unit::unit_type,Y>
atan(const quantity<unit<dimensionless_type, heterogeneous_dimensionless_system>, Y>& val)
{
    using std::atan;
    return quantity<angle::radian_base_unit::unit_type,Y>::from_value(atan(val.value()));
}

/// atan2 of @c value_type returning angle in radians
template<class Y, class Dimension, class System>
quantity<unit<plane_angle_dimension, homogeneous_system<System> >, Y>
atan2(const quantity<unit<Dimension, homogeneous_system<System> >, Y>& y,
      const quantity<unit<Dimension, homogeneous_system<System> >, Y>& x)
{
    using std::atan2;
    return quantity<unit<plane_angle_dimension, homogeneous_system<System> >, Y>(atan2(y.value(),x.value())*si::radians);
}

/// atan2 of @c value_type returning angle in radians
template<class Y, class Dimension, class System>
quantity<angle::radian_base_unit::unit_type,Y>
atan2(const quantity<unit<Dimension, heterogeneous_system<System> >, Y>& y,
      const quantity<unit<Dimension, heterogeneous_system<System> >, Y>& x)
{
    using std::atan2;
    return quantity<angle::radian_base_unit::unit_type,Y>::from_value(atan2(y.value(),x.value()));
}

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CMATH_HPP


/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <ignition/math/Rand.hh>

#include <boost/foreach.hpp>
#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
#include "gazebo/test/helper_physics_generator.hh"

using namespace gazebo;

class TransceiverTest : public ServerFixture,
                        public testing::WithParamInterface<const char*>
{
  public: TransceiverTest();
  public: void TxRxEmptySpace(const std::string &_physicsEngine);
  public: void TxRxObstacle(const std::string &_physicsEngine);
  public: void TxRxFreqOutOfBounds(const std::string &_physicsEngine);
  public: void TxRxStrictUpdateRate(const std::string &_physicsEngine);
  private: void RxMsg(const ConstWirelessNodesPtr &_msg);
  private: void OnNewUpdate(int* _msgCounter);

  private: static const double MinFreq;
  private: static const double MaxFreq;
  private: static const double Gain;
  private: static const double Power;
  private: static const double Sensitivity;
  private: static const double MaxPos;

  private: std::mutex mutex;
  private: std::vector<int> num_msgs;
  private: std::vector<common::Time> elapsed_time;
  private: boost::shared_ptr<msgs::WirelessNodes const> nodesMsg;
  private: bool receivedMsg;
};

const double TransceiverTest::MinFreq = 2412.0;
const double TransceiverTest::MaxFreq = 2484.0;
const double TransceiverTest::Gain = 2.6;
const double TransceiverTest::Power = 14.5;
const double TransceiverTest::Sensitivity = -90.0;
const double TransceiverTest::MaxPos = 10.0;

/////////////////////////////////////////////////
TransceiverTest::TransceiverTest()
{
  this->receivedMsg = false;
}

/////////////////////////////////////////////////
void TransceiverTest::RxMsg(const ConstWirelessNodesPtr &_msg)
{
  std::lock_guard<std::mutex> lock(mutex);
  // Just copy the message
  this->nodesMsg = _msg;
  this->receivedMsg = true;
}

/////////////////////////////////////////////////
void TransceiverTest::OnNewUpdate(int* _msgCounter)
{
  *_msgCounter += 1;
}

/////////////////////////////////////////////////
void TransceiverTest::TxRxEmptySpace(const std::string &_physicsEngine)
{
  typedef std::map<std::string, sensors::WirelessTransmitterPtr> trans_map_type;
  trans_map_type transmitters;

  Load("worlds/empty.world", true, _physicsEngine);

  // Generate a random number [1-10] of transmitters
  int nTransmitters = ignition::math::Rand::IntUniform(1, 10);

  for (int i = 0; i < nTransmitters; ++i)
  {
    double txFreq = ignition::math::Rand::DblUniform(this->MinFreq,
        this->MaxFreq);
    std::ostringstream convert;
    convert << i;
    std::string txModelName = "tx" + convert.str();
    std::string txSensorName = "wirelessTransmitter" + convert.str();
    std::string txEssid = "osrf" + convert.str();
    double x = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
    double y = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
    ignition::math::Pose3d txPose(ignition::math::Vector3d(x, y, 0.055),
        ignition::math::Quaterniond::Identity);

    SpawnWirelessTransmitterSensor(txModelName, txSensorName, txPose.Pos(),
        txPose.Rot().Euler(), txEssid, txFreq, this->Power, this->Gain);

    sensors::WirelessTransmitterPtr tx =
        std::static_pointer_cast<sensors::WirelessTransmitter>(
          sensors::SensorManager::Instance()->GetSensor(txSensorName));

    // Store the new transmitter sensor in the map
    transmitters[txEssid] = tx;

    ASSERT_TRUE(tx != NULL);
  }

  // Wireless Receiver - rx
  std::string rxModelName = "rx";
  std::string rxSensorName = "wirelessReceiver";
  ignition::math::Pose3d rxPose(ignition::math::Vector3d(0, 2, 0.055),
      ignition::math::Quaterniond::Identity);

  // Spawn rx
  SpawnWirelessReceiverSensor(rxModelName, rxSensorName, rxPose.Pos(),
      rxPose.Rot().Euler(), this->MinFreq, this->MaxFreq, this->Power,
      this->Gain, this->Sensitivity);

  sensors::WirelessReceiverPtr rx =
    std::static_pointer_cast<sensors::WirelessReceiver>(
        sensors::SensorManager::Instance()->GetSensor(rxSensorName));

  ASSERT_TRUE(rx != NULL);

  // Initialize gazebo transport layer
  transport::NodePtr node(new transport::Node());
  node->Init("default");

  std::string rxTopic = "/gazebo/default/rx/link/wirelessReceiver/transceiver";
  transport::SubscriberPtr sub = node->Subscribe(rxTopic,
      &TransceiverTest::RxMsg, this);
  this->receivedMsg = false;

  // Loop a max. of ~5 seconds
  for (int i = 0; i < 50; ++i)
  {
    // Update all the transmitter sensors
    BOOST_FOREACH(const trans_map_type::value_type& myPair, transmitters)
    {
      myPair.second->Update(true);
    }

    // Update the receiver sensor
    rx->Update(true);

    common::Time::MSleep(100);
    std::lock_guard<std::mutex> lock(this->mutex);

    if (this->nodesMsg && this->receivedMsg)
    {
      this->receivedMsg = false;
      gazebo::msgs::WirelessNodes txNodes;
      int numTxNodes = nodesMsg->node_size();

      for (int i = 0; i < numTxNodes; ++i)
      {
        gazebo::msgs::WirelessNode txNode = nodesMsg->node(i);
        std::string essid = txNode.essid();
        EXPECT_EQ(transmitters[essid]->ESSID(), essid);
        EXPECT_EQ(transmitters[essid]->Freq(), txNode.frequency());
        EXPECT_LE(txNode.signal_level(), 0);
        EXPECT_GE(txNode.signal_level(), rx->Sensitivity());
      }
      return;
    }
  }
  FAIL();
}

/////////////////////////////////////////////////
void TransceiverTest::TxRxFreqOutOfBounds(const std::string &_physicsEngine)
{
  Load("worlds/empty.world", true, _physicsEngine);

  double txFreq = this->MinFreq - 1.0;
  std::string tx1ModelName = "tx1";
  std::string tx1SensorName = "wirelessTransmitter1";
  std::string tx2ModelName = "tx2";
  std::string tx2SensorName = "wirelessTransmitter2";
  std::string txEssid = "osrf";
  double x = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
  double y = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
  ignition::math::Pose3d txPose(ignition::math::Vector3d(x, y, 0.055),
                                ignition::math::Quaterniond::Identity);

  SpawnWirelessTransmitterSensor(tx1ModelName, tx1SensorName, txPose.Pos(),
      txPose.Rot().Euler(), txEssid, txFreq, this->Power, this->Gain);

  sensors::WirelessTransmitterPtr tx1 =
      std::static_pointer_cast<sensors::WirelessTransmitter>(
        sensors::SensorManager::Instance()->GetSensor(tx1SensorName));

  ASSERT_TRUE(tx1 != NULL);

  txFreq = this->MaxFreq + 1.0;
  SpawnWirelessTransmitterSensor(tx2ModelName, tx2SensorName, txPose.Pos(),
      txPose.Rot().Euler(), txEssid, txFreq, this->Power, this->Gain);

  sensors::WirelessTransmitterPtr tx2 =
      std::static_pointer_cast<sensors::WirelessTransmitter>(
        sensors::SensorManager::Instance()->GetSensor(tx2SensorName));

  ASSERT_TRUE(tx2 != NULL);

  // Wireless Receiver - rx
  std::string rxModelName = "rx";
  std::string rxSensorName = "wirelessReceiver";
  ignition::math::Pose3d rxPose(ignition::math::Vector3d(0, 2, 0.055),
      ignition::math::Quaterniond::Identity);

  // Spawn rx
  SpawnWirelessReceiverSensor(rxModelName, rxSensorName, rxPose.Pos(),
      rxPose.Rot().Euler(), this->MinFreq, this->MaxFreq, this->Power,
      this->Gain, this->Sensitivity);

  sensors::WirelessReceiverPtr rx =
    std::static_pointer_cast<sensors::WirelessReceiver>(
        sensors::SensorManager::Instance()->GetSensor(rxSensorName));

  ASSERT_TRUE(rx != NULL);

  // Initialize gazebo transport layer
  transport::NodePtr node(new transport::Node());
  node->Init("default");

  std::string rxTopic = "/gazebo/default/rx/link/wirelessReceiver/transceiver";
  transport::SubscriberPtr sub = node->Subscribe(rxTopic,
      &TransceiverTest::RxMsg, this);
  this->receivedMsg = false;

  // Loop a max. of ~5 seconds
  for (int i = 0; i < 50; ++i)
  {
    // Update the sensors
    tx1->Update(true);
    tx2->Update(true);
    rx->Update(true);

    common::Time::MSleep(100);
    std::lock_guard<std::mutex> lock(this->mutex);
  }

  EXPECT_FALSE(this->receivedMsg);
}

/////////////////////////////////////////////////
void TransceiverTest::TxRxObstacle(const std::string &_physicsEngine)
{
  Load("worlds/empty.world", true, _physicsEngine);

  double avgSignalLevelEmpty = 0.0;
  double avgSignalLevelObstacle = 0.0;
  int samples = 0;

  // Wireless Transmitter - tx
  std::string txModelName = "tx";
  std::string txSensorName = "wirelessTx";
  ignition::math::Pose3d txPose(ignition::math::Vector3d(0, 0, 0.5),
      ignition::math::Quaterniond::Identity);

  // Spawn tx
  SpawnWirelessTransmitterSensor(txModelName, txSensorName, txPose.Pos(),
      txPose.Rot().Euler(), "osrf", 2450.0, this->Power, this->Gain);

  sensors::WirelessTransmitterPtr tx =
      std::static_pointer_cast<sensors::WirelessTransmitter>(
        sensors::SensorManager::Instance()->GetSensor(txSensorName));

  ASSERT_TRUE(tx != NULL);

  // Wireless Receiver - rx1
  std::string rx1ModelName = "rx1";
  std::string rx1SensorName = "wirelessRx1";
  ignition::math::Pose3d rx1Pose(ignition::math::Vector3d(3, 0, 0.5),
      ignition::math::Quaterniond::Identity);

  // Spawn rx1
  SpawnWirelessReceiverSensor(rx1ModelName, rx1SensorName, rx1Pose.Pos(),
      rx1Pose.Rot().Euler(), this->MinFreq, this->MaxFreq, this->Power,
      this->Gain, this->Sensitivity);

  sensors::WirelessReceiverPtr rx1 =
      std::static_pointer_cast<sensors::WirelessReceiver>(
        sensors::SensorManager::Instance()->GetSensor(rx1SensorName));

  ASSERT_TRUE(rx1 != NULL);

  // Wireless Receiver - rx2
  std::string rx2ModelName = "rx2";
  std::string rx2SensorName = "wirelessRx2";
  ignition::math::Pose3d rx2Pose(ignition::math::Vector3d(-2, 0, 0.5),
                                 ignition::math::Quaterniond::Identity);
  // Spawn rx2
  SpawnWirelessReceiverSensor(rx2ModelName, rx2SensorName, rx2Pose.Pos(),
      rx2Pose.Rot().Euler(), this->MinFreq, this->MaxFreq, this->Power,
      this->Gain, this->Sensitivity);

  sensors::WirelessReceiverPtr rx2 =
      std::static_pointer_cast<sensors::WirelessReceiver>(
        sensors::SensorManager::Instance()->GetSensor(rx2SensorName));

  ASSERT_TRUE(rx2 != NULL);

  // Spawn an obstacle between the transmitter and the receiver
  SpawnBox("Box", ignition::math::Vector3d(1, 1, 1),
                  ignition::math::Vector3d(-1, 0, 0.5),
                  ignition::math::Vector3d::Zero, true);
  // Initialize gazebo transport layer
  transport::NodePtr node(new transport::Node());
  node->Init("default");

  std::string rx1Topic = "/gazebo/default/rx1/link/wirelessRx1/transceiver";
  transport::SubscriberPtr sub = node->Subscribe(rx1Topic,
      &TransceiverTest::RxMsg, this);

  this->receivedMsg = false;

  // Loop until you have 10 samples or timeout after ~5secs
  int iters = 0;
  while (samples < 10 && iters < 50)
  {
    // Update all the sensors
    tx->Update(true);
    rx1->Update(true);

    common::Time::MSleep(100);
    std::lock_guard<std::mutex> lock(this->mutex);

    if (this->nodesMsg && this->receivedMsg)
    {
      this->receivedMsg = false;
      gazebo::msgs::WirelessNodes txNodes;
      int numTxNodes = nodesMsg->node_size();

      for (int i = 0; i < numTxNodes; ++i)
      {
        gazebo::msgs::WirelessNode txNode = nodesMsg->node(i);
        double signalLevel = txNode.signal_level();

        ++samples;
        avgSignalLevelEmpty += signalLevel;
      }
    }
    ++iters;
  }

  samples = 0;
  iters = 0;

  std::string rx2Topic = "/gazebo/default/rx2/link/wirelessRx2/transceiver";
  sub = node->Subscribe(rx2Topic, &TransceiverTest::RxMsg, this);

  this->receivedMsg = false;

  // Loop until you have 10 samples or timeout after ~5secs
  while (samples < 10 && iters < 50)
  {
    // Update all the sensors
    tx->Update(true);
    rx2->Update(true);

    common::Time::MSleep(100);
    std::lock_guard<std::mutex> lock(this->mutex);

    if (this->nodesMsg && this->receivedMsg)
    {
      this->receivedMsg = false;
      gazebo::msgs::WirelessNodes txNodes;
      int numTxNodes = nodesMsg->node_size();

      for (int i = 0; i < numTxNodes; ++i)
      {
        gazebo::msgs::WirelessNode txNode = nodesMsg->node(i);
        double signalLevel = txNode.signal_level();

        ++samples;
        avgSignalLevelObstacle += signalLevel;
      }
    }
    ++iters;
  }

  // Check that the signal level in the not-occluded receiver is higher than
  // the signal received by the occluded receiver
  EXPECT_GT(avgSignalLevelEmpty / samples, avgSignalLevelObstacle / samples);
}

/////////////////////////////////////////////////
void TransceiverTest::TxRxStrictUpdateRate(const std::string &_physicsEngine)
{
  LoadArgs(" --lockstep -u -e " + _physicsEngine +
      " worlds/transceiver_strict_rate.world");

  // Wait until the sensors have been initialized
  while (!sensors::SensorManager::Instance()->SensorsInitialized())
    common::Time::MSleep(1000);

  std::string txSensorName = "tx_sensor";
  sensors::SensorPtr txSensor = sensors::get_sensor(txSensorName);
  sensors::WirelessTransmitterPtr tx =
      std::dynamic_pointer_cast<sensors::WirelessTransmitter>(txSensor);
  ASSERT_TRUE(tx != NULL);

  std::string rxSensorName = "rx_sensor";
  sensors::SensorPtr rxSensor = sensors::get_sensor(rxSensorName);
  sensors::WirelessReceiverPtr rx =
      std::dynamic_pointer_cast<sensors::WirelessReceiver>(rxSensor);
  ASSERT_TRUE(rx != NULL);

  // Connect update callback and start timer
  int msgCount = 0;
  event::ConnectionPtr c = rx->ConnectUpdated(
      std::bind(&TransceiverTest::OnNewUpdate, this, &msgCount));
  common::Timer timer;
  SetPause(false);
  timer.Start();

  // how many msgs produced for 5 seconds (in simulated clock domain)
  double updateRate = rx->UpdateRate();
  int totalMsgs = 5 * updateRate;
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);
  double simT0 = 0.0;

  while (msgCount < totalMsgs)
  {
    // An approximation of when we receive the first image. In reality one
    // iteration before we receive the second image.
    if (msgCount == 0)
    {
      simT0 = world->SimTime().Double();
    }
    common::Time::MSleep(1);
  }

  // check that the obtained rate is the one expected
  double dt = world->SimTime().Double() - simT0;
  double rate = static_cast<double>(totalMsgs) / dt;
  gzdbg << "timer [" << dt << "] seconds rate [" << rate << "] fps\n";
  const double tolerance = 0.02;
  EXPECT_GT(rate, updateRate * (1 - tolerance));
  EXPECT_LT(rate, updateRate * (1 + tolerance));
  c.reset();
}

/////////////////////////////////////////////////
TEST_P(TransceiverTest, EmptyWorld)
{
  TxRxEmptySpace(GetParam());
}

/////////////////////////////////////////////////
TEST_P(TransceiverTest, Obstacle)
{
  if (std::string(GetParam()) == "simbody")
  {
    gzerr << "Abort test since this test frequently fails with simbody, "
          << " see (issues #867)" << std::endl;
    return;
  }
  if (std::string(GetParam()) == "dart")
  {
    gzerr << "Abort test since this test frequently fails with dart, "
          << " see (issues #911)" << std::endl;
    return;
  }

  TxRxObstacle(GetParam());
}

/////////////////////////////////////////////////
TEST_P(TransceiverTest, FreqOutOfBounds)
{
  TxRxFreqOutOfBounds(GetParam());
}

/////////////////////////////////////////////////
TEST_P(TransceiverTest, StrictUpdateRate)
{
  TxRxStrictUpdateRate(GetParam());
}

/////////////////////////////////////////////////
INSTANTIATE_TEST_CASE_P(PhysicsEngines, TransceiverTest,
                        PHYSICS_ENGINE_VALUES,);  // NOLINT

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


/*
Copyright 2016 Australian Centre for Robotic Vision
*/

#include "tf/transform_datatypes.h"
#include <pcl/filters/passthrough.h>
#include <math.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <apc_3d_vision/split_labelled_point_cloud.h>
#include <apc_3d_vision.hpp>
#include <eigen_conversions/eigen_msg.h>
#include "tf_conversions/tf_eigen.h"
#include <map>
#include <utility>
#include <vector>
#include <stdio.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
#include <apc_msgs/CropCloud.h>
#include <pcl/filters/statistical_outlier_removal.h>
// global value to listen to transforms
boost::shared_ptr<tf::TransformListener> tf_listener_ptr;
tf::Transform tftote;
Apc3dVision apc_vis;
ros::Publisher debug1;
ros::Publisher debug2;

// Set a global value to us PI in shorthand
#define PI 3.14159265
// values to help with Calibration
int value = 0;
int valueMax = 255;
bool CALIBRATE = false;
bool publishTF = false;
// value to set calibration window name
static const std::string Window = "CALIBRATE windows";
// The results from calibration
int Hlow; // = 20; //20
int Hhigh; // = 40; //32
int Slow; // = 0;
int Shigh; // = 0;
int Vlow; // = 255;
int Vhigh; // = 255;

bool Hinside;

float Xmin;
float Xmax;
float Ymin;
float Ymax;
float Zmin;
float Zmax;

int img_top;
int img_bottom;
int img_left;
int img_right;

bool lookupTransform(const std::string &fromFrame, const std::string &toFrame,
                     tf::StampedTransform &foundTransform) {
  try {
    ros::Time now = ros::Time::now();
    ros::Time zero = ros::Time(0);
    if (!tf_listener_ptr)
      ROS_ERROR("asdfsdaf");

    bool tf_success = tf_listener_ptr->waitForTransform(
        fromFrame, toFrame, zero, ros::Duration(3.0));

    if (tf_success) {
      tf_listener_ptr->lookupTransform(fromFrame, toFrame, zero,
                                       foundTransform);
    } else {
      ROS_WARN("Could not lookup transform.");
    }
  } catch (tf::TransformException ex) {
    ROS_ERROR("TransformException: %s", ex.what());
    return false;
  } catch (...) {
    ROS_ERROR("Unknown exception.");
    return false;
  }
  return true;
}
// Callback functions to set the values of the calibration
void ChangeHlow(int, void *) { Hlow = uint8_t(value); }
void ChangeSlow(int, void *) { Slow = uint8_t(value); }
void ChangeVlow(int, void *) { Vlow = uint8_t(value); }
void ChangeHhigh(int, void *) { Hhigh = uint8_t(value); }
void ChangeShigh(int, void *) { Shigh = uint8_t(value); }
void ChangeVhigh(int, void *) { Vhigh = uint8_t(value); }

bool split_labelled_point_cloud(apc_msgs::CropCloud::Request &req,
                                apc_msgs::CropCloud::Response &res) {
  ros::NodeHandle nh("~");

  bool crop_image;
  try {
    crop_image = req.crop_image.data;
  } catch (...) {
    crop_image = false;
  }

  bool crop_cloud;
  try {
    crop_cloud = req.crop_cloud.data;
  } catch (...) {
    crop_cloud = false;
  }

  std::string storage_name = req.storage_name.data;

  if(crop_cloud) {
    nh.getParam(std::string("/storage_specs/") + storage_name + "/h_low", Hlow);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/h_high", Hhigh);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/s_low", Slow);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/s_high", Shigh);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/v_low", Vlow);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/v_high", Vhigh);

    nh.getParam(std::string("/storage_specs/") + storage_name + "/h_inside", Hinside);

    nh.getParam(std::string("/storage_specs/") + storage_name + "/x_max", Xmax);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/x_min", Xmin);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/y_max", Ymax);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/y_min", Ymin);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/z_max", Zmax);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/z_min", Zmin);

    // Get the pointcloud out of the Request
    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> req_points;
    req_points.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    try {
      pcl::fromROSMsg(req.input_cloud, *req_points);
    } catch (...) {
      // std::cout << "There is now points in this message!" << std::endl;
      res.success.data = false;
      return false;
    }

    int kept = 0;
    int removed = 0;
    int outside = 0;

    pcl::PointCloud<pcl::PointXYZRGB> croppedCloud_outlier;
    pcl::PointCloud<pcl::PointXYZRGB> croppedCloud_inlier;
    pcl::PointCloud<pcl::PointXYZRGB> croppedCloud_posonly;
    if (CALIBRATE) {
      cv::Mat Color_cropped = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0, 0, 0));
      int i = 0;
      BOOST_FOREACH (pcl::PointXYZRGB point, *req_points) {
        i++;
        if (point.r == 0) {
          continue;
        }
        cv::Mat M = cv::Mat(1, 1, CV_8UC3, cv::Scalar(point.r, point.g, point.b));
        cv::Mat M_hsv;
        cvtColor(M, M_hsv, CV_RGB2HSV);
        if (((M_hsv.data[0] > Hlow) && (M_hsv.data[0] < Hhigh)) &&
            ((M_hsv.data[1] < Slow) && (M_hsv.data[1] < Shigh)) &&
            ((M_hsv.data[2] > Vlow) && (M_hsv.data[2] < Vhigh))) {
          croppedCloud_outlier.push_back(point);
          Color_cropped.data[3 * i + 2] = point.r;
          Color_cropped.data[3 * i + 1] = point.g;
          Color_cropped.data[3 * i] = point.b;
        }
      }

    } else {

      pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
      sor.setMeanK(25);
      sor.setStddevMulThresh(1.0);
      sor.setInputCloud(req_points);
      sor.filter(*req_points);

      BOOST_FOREACH (pcl::PointXYZRGB point, *req_points) {
      //for(auto &point: *req_points) {
        if (point.r == 0) {
          continue;
        }
        cv::Mat M = cv::Mat(1, 1, CV_8UC3, cv::Scalar(point.r, point.g, point.b));
        cv::Mat M_hsv;
        cvtColor(M, M_hsv, CV_RGB2HSV);
        if (
          (point.x < Xmin) || (point.x > Xmax) ||
          (point.y < Ymin) || (point.y > Ymax) ||
          (point.z < Zmin) || (point.z > Zmax)
        ) {
          outside++;
        }
        else if (
            ((Hinside && ((M_hsv.data[0] > Hlow) && (M_hsv.data[0] < Hhigh))) ||
            (!Hinside && ((M_hsv.data[0] < Hlow) || (M_hsv.data[0] > Hhigh)))) &&
            ((M_hsv.data[1] > Slow) && (M_hsv.data[1] < Shigh)) &&
            ((M_hsv.data[2] > Vlow) && (M_hsv.data[2] < Vhigh))
        ) {
          // Keep the point
          croppedCloud_posonly.push_back(point);
          croppedCloud_inlier.push_back(point);
          kept++;
        } else {
          // Remove the point
          croppedCloud_posonly.push_back(point);
          croppedCloud_outlier.push_back(point);
          removed++;
        }
      }
      std::cout << "Cropping Tote" << std::endl;
    }

    sensor_msgs::PointCloud2 ros_cloud_inliers;
    sensor_msgs::PointCloud2 ros_cloud_outliers;
    sensor_msgs::PointCloud2 ros_cloud_posonly;
    try {
      pcl::toROSMsg(croppedCloud_inlier, ros_cloud_inliers);
      pcl::toROSMsg(croppedCloud_outlier, ros_cloud_outliers);
      pcl::toROSMsg(croppedCloud_posonly, ros_cloud_posonly);
      ros_cloud_inliers.header.stamp = ros::Time::now();
      ros_cloud_inliers.header.frame_id = req.input_cloud.header.frame_id;
      ros_cloud_outliers.header.stamp = ros::Time::now();
      ros_cloud_outliers.header.frame_id = req.input_cloud.header.frame_id;
      ros_cloud_posonly.header.stamp = ros::Time::now();
      ros_cloud_posonly.header.frame_id = req.input_cloud.header.frame_id;
      res.segmented_cloud_inliers = ros_cloud_inliers;
      res.segmented_cloud_outliers = ros_cloud_outliers;
      res.segmented_cloud_posonly = ros_cloud_posonly;
    }
    catch (...) {
      std::cout << "You filtered everything and there's nothing in this cloud!"
                << std::endl;
      res.success.data = false;
      return false;
    }
  }

  if(crop_image) {
    nh.getParam(std::string("/storage_specs/") + storage_name + "/img_top", img_top);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/img_bottom", img_bottom);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/img_left", img_left);
    nh.getParam(std::string("/storage_specs/") + storage_name + "/img_right", img_right);

    // Load the image.
    cv_bridge::CvImagePtr image_ptr;
    cv::Mat img;
    try{
        image_ptr = cv_bridge::toCvCopy(req.image, "");
        img = image_ptr->image;
    }
    catch (cv_bridge::Exception& e){
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }

    // Load the Depth
    cv_bridge::CvImagePtr depth_ptr;
    cv::Mat depth;
    try{
        depth_ptr = cv_bridge::toCvCopy(req.depth, "");
        depth = depth_ptr->image;
    }
    catch (cv_bridge::Exception& e){
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }

    // Create a rectangular mask.
    cv::Mat mask = cv::Mat::zeros(img.size(), CV_8U);
    mask(cv::Rect(img_left, img_top, img_right-img_left, img_bottom-img_top)) = 1;

    // Crop the image.
    cv::Mat cropped_img = cv::Mat::zeros(img.size(), img.type());
    img.copyTo(cropped_img, mask);
    image_ptr->image = cropped_img;
    image_ptr->toImageMsg(res.cropped_image);

    // Crop the Depth.
    cv::Mat cropped_depth = cv::Mat::zeros(depth.size(), depth.type());
    depth.copyTo(cropped_depth, mask);
    depth_ptr->image = cropped_depth;
    depth_ptr->toImageMsg(res.cropped_depth);

  }

  res.success.data = true;
  return true;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "cropTote");
  ros::NodeHandle nh_("~");
  // transform broadcaster to broadcast the recieved TF
  tf::TransformBroadcaster br;
  // Advertise the service to crop the cloud
  ros::ServiceServer service = nh_.advertiseService(
      "/apc_3d_vision/Crop_Tote_cloud", &split_labelled_point_cloud);
  tf_listener_ptr.reset(new tf::TransformListener());

  debug1 = nh_.advertise<sensor_msgs::PointCloud2>("/debug1", 1);
  ;
  debug2 = nh_.advertise<sensor_msgs::PointCloud2>("/debug2", 1);
  ;

  ros::AsyncSpinner spinner(2);
  spinner.start();
  if (CALIBRATE) {
    cv::namedWindow(Window);
    // char TrackbarName[50];
    // sprintf(TrackbarName, "Hlow %d", valueMax);
    cv::createTrackbar("H>Hlow", Window, &value, valueMax, ChangeHlow);

    // char TrackbarName2[50];
    // sprintf(TrackbarName2, "Slow %d", valueMax);
    cv::createTrackbar("S>Slow", Window, &value, valueMax, ChangeSlow);
    // char TrackbarName3[50];
    // sprintf(TrackbarName3, "Vlow %d", valueMax);
    cv::createTrackbar("V>Vlow", Window, &value, valueMax, ChangeVlow);

    // char TrackbarName4[50];
    // sprintf(TrackbarName4, "Hhigh %d", valueMax);
    cv::createTrackbar("Hhigh<H", Window, &value, valueMax, ChangeHhigh);
    // char TrackbarName5[50];
    // sprintf(TrackbarName5, "Shigh %d", valueMax);
    cv::createTrackbar("Shigh<S", Window, &value, valueMax, ChangeShigh);
    // char TrackbarName6[50];
    // sprintf(TrackbarName6, "Vhigh %d", valueMax);
    cv::createTrackbar("Vhigh<V", Window, &value, valueMax, ChangeVhigh);
  }
  ros::Rate rosrate(10);
  while (ros::ok()) {
    // if (publishTF)
      // br.sendTransform(tf::StampedTransform(tftote, ros::Time::now(), "torso",
      //                                       "totePosition"));
    rosrate.sleep();
  }

  return 0;
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PipelineRecomputeNumericalTest0

#include <../random_util.hpp>
#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <map>
#include <tuple>
#include <vector>

#define protected public
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/ipucopy.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/restore.hpp>
#include <popart/op/stash.hpp>
#include <popart/optimizer.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>
#undef protected
#include <popart/session.hpp>

BOOST_AUTO_TEST_CASE(PipelineRecomputeNumericalTest0x) {

  // Model :
  //
  //  Input         Weights
  //     |            |
  //     +--- mul ----+
  //           |
  //         sigmoid
  //           |
  //          scale
  //           |            IPU 0
  // - -  - -  - -  - -  - -  - -  - -  - -  - -  - -
  //           |            IPU 1
  //          L1 loss
  //
  //
  //  Without recomputation, both Input and output of sigmoid are stashed.
  //  Note that Sigmoid grad uses the output of sigmoid - this is whyoutput if
  //  sigmoid must be kept.
  //
  //  With recomputation, the output of sigmoid is not stashed, but recomputed
  //  in the backwards pass.
  //
  //
  //
  //
  //  Test :
  //
  //  Using gradient accumulation (3 microbatches accumulated, 1 batch per step)
  //  We confirm that
  //  1) no pipelining
  //  2) pipelining without recomputation
  //  3) pipelining with recomputation
  //  all produce exactly the same final model (after training for 2 steps)
  //
  //  We also confirm that the weights have actually changed, and that they are
  //  not fully converged

  // Store the initial and final weights from training
  class WeightPair {
  public:
    using Wts = std::vector<std::vector<float>>;
    WeightPair(Wts s, Wts e) : start(s), end(e) {}
    Wts start;
    Wts end;
  };

  enum class RunType {
    SingleDevice = 0,    // exact SGD on a single device
    ContinuousPipe,      // Continuous (in-exact) pipelining
    ContinuousRecompPipe // Continuous pipelined
  };

  using namespace popart;

  auto getResult = [](RunType rt) {
    int accumulationFactor  = 3;
    int microBatchesPerStep = 3;
    if (microBatchesPerStep % accumulationFactor != 0) {
      throw error("accumulation factor is not a factor of microBatchesPerStep");
    }
    int64_t batchesPerStep = microBatchesPerStep / accumulationFactor;

    int seed = 1011;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-1.f, +1.f);

    int64_t microBatchSize = 2;
    int64_t sampleHeight   = 8;
    std::vector<int64_t> sampleShape{sampleHeight, sampleHeight};
    std::vector<int64_t> weightShape = sampleShape;
    std::vector<int64_t> microBatchShape{
        microBatchSize, sampleHeight, sampleHeight};
    TensorInfo sampleInfo{"FLOAT", sampleShape};
    TensorInfo weightInfo = sampleInfo;
    TensorInfo microBatchInfo{"FLOAT", microBatchShape};
    int64_t sampleElms{sampleHeight * sampleHeight};
    int64_t microBatchElms = sampleElms * microBatchSize;

    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();
    auto input0      = builder->addInputTensor(microBatchInfo, "0tupni");

    WeightsIO weights_io_read;
    int nIPUs = (rt != RunType::SingleDevice ? 2 : 1);

    auto weights_readback = std::vector<float>(sampleElms, -99.0f);
    auto weights_init     = std::vector<float>(sampleElms, 0);
    for (auto &x : weights_init) {
      x = 1 * fdis(eng);
    }
    ConstVoidData weights_cvd = {weights_init.data(), sampleInfo};
    TensorId weights_id       = builder->addInitializedInputTensor(weights_cvd);

    auto act0 = aiOnnx.mul({weights_id, input0});
    builder->virtualGraph(act0, 0);

    act0 = aiOnnx.sigmoid({act0});
    builder->virtualGraph(act0, 0);

    act0 = aiGraphcore.scale({act0}, 2.0f);
    builder->virtualGraph(act0, 0);

    weights_io_read.insert(weights_id, {weights_readback.data(), weightInfo});

    TensorId actFinal = act0;

    SessionOptions userOptions;

    userOptions.virtualGraphMode = VirtualGraphMode::Manual;

    userOptions.reportOptions.insert({"showExecutionSteps", "true"});

    if (rt != RunType::SingleDevice) {
      userOptions.enablePipelining = true;
    }

    if (rt == RunType::ContinuousRecompPipe) {
      userOptions.autoRecomputation = RecomputationType::Standard;
    }

    if (accumulationFactor > 1) {
      userOptions.accumulationFactor         = accumulationFactor;
      userOptions.enableGradientAccumulation = true;
    }

    // Changing this to SGD does not work, learning rate is not correct I think.
    auto optimizer = ConstSGD(0.04);

    float lambda = 0.159;
    actFinal     = builder->aiGraphcoreOpset1().l1loss({actFinal}, lambda);
    builder->virtualGraph(actFinal, nIPUs - 1);

    auto proto    = builder->getModelProto();
    auto dataFlow = DataFlow(batchesPerStep);

    int64_t stepDataElms = accumulationFactor * microBatchElms * batchesPerStep;

    auto device = createTestDevice(TEST_TARGET, nIPUs);

    auto session = popart::TrainingSession::createFromOnnxModel(
        proto,
        dataFlow,
        actFinal,
        optimizer,
        device,
        InputShapeInfo(),
        userOptions,
        popart::Patterns(PatternsLevel::Default));

    auto opSchedule =
        session->getIr().getOpSchedule({}, RequireOptimalSchedule::Yes);
    int nRestore = 0;
    for (auto op : opSchedule) {
      // number of restores
      if (dynamic_cast<RestoreOp *>(op) ||
          dynamic_cast<RestoreInplaceOp *>(op)) {
        ++nRestore;
      }
    }
    if (rt == RunType::SingleDevice) {
      BOOST_CHECK(nRestore == 0);
    } else if (rt == RunType::ContinuousRecompPipe) {
      BOOST_CHECK(nRestore == 1);
    } else {
      BOOST_CHECK(rt == RunType::ContinuousPipe);
      BOOST_CHECK(nRestore == 2);
    }

    session->prepareDevice();

    std::vector<float> v_input_x(stepDataElms);
    std::map<popart::TensorId, popart::IArray &> anchors = {};

    std::vector<WeightPair> weightPairs;

    eng.seed(57);

    auto nSteps = 2;

    // initialize weights, write weights to device
    session->resetHostWeights(proto);
    session->weightsFromHost();

    int64_t samplesPerStep =
        batchesPerStep * accumulationFactor * microBatchSize;

    std::vector<int64_t> stepDataShape{batchesPerStep * accumulationFactor,
                                       microBatchSize,
                                       sampleHeight,
                                       sampleHeight};

    TensorInfo stepDataInfo{"FLOAT", stepDataShape};

    for (int i = 0; i < nSteps; ++i) {
      std::cout << "Step # " << i << std::endl;

      // generate new samples
      for (int i = 0; i < samplesPerStep; ++i) {
        for (int j = 0; j < sampleElms; ++j) {
          auto stepIndex       = i * sampleElms + j;
          v_input_x[stepIndex] = fdis(eng);
        }
      }
      popart::NDArrayWrapper<float> input_x_wrapper(v_input_x.data(),
                                                    stepDataInfo);
      std::map<popart::TensorId, popart::IArray &> inputs = {
          {input0, input_x_wrapper}};
      popart::StepIO stepio(inputs, anchors);

      // process the samples
      session->run(stepio);

      if (i == 0 && rt == RunType::ContinuousRecompPipe) {
        auto summaryReport = session->getSummaryReport();
        std::ofstream filly;
        filly.open("recompLog.log");
        filly << summaryReport;
        filly.close();
      }

      if (i == 0 && rt == RunType::ContinuousPipe) {
        auto summaryReport = session->getSummaryReport();
        std::ofstream filly;
        filly.open("pipelineLog.log");
        filly << summaryReport;
        filly.close();
      }
    }

    // read final weights back
    session->weightsToHost();
    session->readWeights(weights_io_read);
    return WeightPair({weights_init}, {weights_readback});
  };

  auto getDelta = [](const std::vector<std::vector<float>> &a,
                     const std::vector<std::vector<float>> &b) {
    std::vector<float> vDelta;
    for (int layer = 0; layer < a.size(); ++layer) {
      float delta = 0;
      for (int i = 0; i < a[layer].size(); ++i) {
        delta += std::abs(a[layer][i] - b[layer][i]);
      }
      vDelta.push_back(delta);
    }
    return vDelta;
  };

  auto print = [&getDelta](const WeightPair &exact,
                           const WeightPair &continuous) {
    auto delta_exact      = getDelta(exact.start, exact.end);
    auto delta_continuous = getDelta(continuous.start, continuous.end);
    auto delta_starts     = getDelta(continuous.start, exact.start);
    auto delta_ends       = getDelta(continuous.end, exact.end);

    std::cout << "(i) | exact end - exact start |_1    (ii) | exact_end - "
                 "continuous_end |_1 / | exact_end - exact_start |_1 "
              << std::endl;
    for (int i = 0; i < delta_exact.size(); ++i) {
      std::cout << "(i) " << delta_exact[i] << "     (ii) "
                << delta_ends[i] / delta_exact[i] << std::endl;
    }
  };

  auto getMeanRelative = [&getDelta](const WeightPair &wp0,
                                     const WeightPair &wp1) {
    float sum       = 0.0f;
    auto delta_0    = getDelta(wp0.start, wp0.end);
    auto delta_ends = getDelta(wp0.end, wp1.end);
    for (auto i = 0; i < delta_0.size(); ++i) {
      sum += delta_ends[i] / delta_0[i];
    }
    return sum / delta_0.size();
  };

  std::cout << "Get results for Continuous without Recomputation\n\n"
            << std::endl;
  auto continuous = getResult(RunType::ContinuousPipe);
  std::cout << "Get results for Continuous with Recomputation\n\n" << std::endl;
  auto recomp = getResult(RunType::ContinuousRecompPipe);
  std::cout << "Get results for SingleDevice run\n\n" << std::endl;
  auto exact = getResult(RunType::SingleDevice);

  bool printLog = true;

  if (printLog) {
    std::cout << "For continuous (no recompute)" << std::endl;
    print(exact, continuous);

    std::cout << "The relative difference between exact and continuous is "
              << getMeanRelative(exact, continuous) << std::endl;

    std::cout << "The relative difference between exact and recomp is "
              << getMeanRelative(exact, recomp) << std::endl;
  }

  //  the experiment above give 0.11868 for this run
  BOOST_CHECK(getMeanRelative(exact, continuous) < 1e-8);
  BOOST_CHECK(getMeanRelative(exact, recomp) < 1e-8);
}


#include <boost/hana/concept/monad.hpp>


#ifndef META_QUIN_HPP
#define META_QUIN_HPP

#include "MetaQuin/m_blat.hpp"
#include "parsers/parser_fa.hpp"
#include "parsers/parser_tsv.hpp"
#include "MetaQuin/m_histogram.hpp"
#include <boost/algorithm/string/predicate.hpp>

namespace Anaquin
{
    struct DAsssembly
    {
        template <typename T> struct Stats : public MappingStats
        {
            Base mean, min, max;
            Base N20, N50, N80;

            // Total number of bases in contigs
            Base total;

            // Total number of bases in the assembly
            Base sum;

            // List of aligned contigs (it's not all contigs)
            std::map<ContigID, T> contigs;

            // Total number of contigs, whether it's aligned or not
            Counts n = 0;
        };

        struct Contig
        {
            ContigID id;

            // Length of the sequence being assembled
            Base len;

            // Size of the contig in k-mer
            Base k_len;

            // Unnormalized k-mer coverage
            Coverage k_cov;

            // Normalized k-mer coverage
            inline Coverage normalized() const { return k_cov ; }
        };

        struct DenoAssemblyImpl
        {
            virtual SequinID findSeq(const ContigID &) const = 0;
        };

        template <typename T = DAsssembly::Stats<Contig>> static T analyze(const FileName &file, DenoAssemblyImpl *impl)
        {
            T stats;
            Histogram hist;

            ParserFA::parse(file, [&](const ParserFA::Data &x, const ParserProgress &)
            {
                Contig c;

                // Can this contig be mapped to a sequin?
                const auto seqID = impl->findSeq(c.id = x.id);

                if (seqID.empty())
                {
                    stats.nEndo++;

                    // Don't bother if the contig doesn't come from sequins...
                    return;
                }
                else
                {
                    stats.nSeqs++;
                }

                // Size of the config (but not alignment size)
                c.len = x.seq.size();

                // The histogram needs the size of the sequence
                hist.insert(x.seq.size());

                stats.contigs[seqID] = c;

                /*
                 if (blat)
                 {
                 // Eg: "contig-30000000	21269" to "contig-30000000"
                 const auto first = Tokens::first(id, " ");

                 id = blat->aligns.count(first) ? first : "";

                 // Don't bother if the contig isn't defined in the alignment...
                 if (id.empty())
                 {
                 return;
                 }
                 }
                 */
            });

            /*
             * https://github.com/bcgsc/abyss/blob/e58e5a6666e0de0e6bdc15c81fe488f5d83085d1/Common/Histogram.h
             */

            stats.sum   = hist.sum();
            stats.N50   = hist.n50();
            stats.min   = hist.minimum();
            stats.max   = hist.maximum();
            stats.mean  = hist.expectedValue();
            stats.N80   = hist.weightedPercentile(1 - 0.8);
            stats.N20   = hist.weightedPercentile(1 - 0.2);
            stats.total = std::accumulate(stats.contigs.begin(), stats.contigs.end(), 0,
                                          [&](int sum, const std::pair<std::string, Contig> &p)
            {
                return sum + p.second.len;
            });

            return stats;
        }

        /*
         * Analyze a de-novo assembly given the assembly and blat alignment.
         */

        template <typename C = Contig, typename T = DAsssembly::Stats<C>> static T analyze_
                (const FileName &file, const MBlat::Stats *blat, std::function<void (C&)> f)
        {
            T stats;
            Histogram hist;

            ParserFA::parse(file, [&](const ParserFA::Data &data, const ParserProgress &)
            {
                //stats.n++;

                C c;
                c.id = data.id;

                auto id = c.id;

                if (blat)
                {
                    // Eg: "contig-30000000	21269" to "contig-30000000"
                    const auto first = Tokens::first(id, " ");

                    id = blat->aligns.count(first) ? first : "";

                    // Don't bother if the contig isn't defined in the alignment...
                    if (id.empty())
                    {
                        return;
                    }
                }

                // Size of the config
                c.len = data.seq.size();

                // The histogram needs the size of the sequence
                hist.insert(data.seq.size());

                // Allows to apply custom operation
                f(c);

                //stats.contigs[id] = c;
            });

            /*
             * https://github.com/bcgsc/abyss/blob/e58e5a6666e0de0e6bdc15c81fe488f5d83085d1/Common/Histogram.h
             */

            //            stats.sum   = hist.sum();
            //            stats.N50   = hist.n50();
            //            stats.min   = hist.minimum();
            //            stats.max   = hist.maximum();
            //            stats.mean  = hist.expectedValue();
            //            stats.N80   = hist.weightedPercentile(1 - 0.8);
            //            stats.N20   = hist.weightedPercentile(1 - 0.2);
            //            stats.sum   = hist.sum();
            //            stats.total = std::accumulate(stats.contigs.begin(), stats.contigs.end(), 0,
            //                            [&](int sum, const std::pair<std::string, C> &p)
            //                            {
            //                                return sum + p.second.len;
            //                            });
            return stats;
        }
    };

    struct Velvet
    {
        template <typename Stats, typename C> static Stats analyze(const FileName &file,
                                                                   const MBlat::Stats *align = NULL)
        {
            Stats stats;

            /*
             * Read coverage from the contig file. The format looks like:
             *
             *      >NODE_77460_length_31_cov_1.129032
             */

            std::vector<std::string> toks;

            return DAsssembly::analyze_<C, Stats>(file, align, [&](C &node)
            {
                Tokens::split(node.id, "_", toks);

                node.k_len = stoi(toks[3]);
                node.k_cov = stod(toks[toks.size() - 1]) * node.k_len;
            });
        }
    };

    struct RayMeta
    {
        template <typename Stats, typename C> static Stats analyze(const FileName &file,
                                                                   const FileName &contigs,
                                                                   const MBlat::Stats *align)
        {
            std::map<ContigID, KMers> covs, lens;

            if (!contigs.empty())
            {
                ParserTSV::parse(Reader(contigs), [&](const ParserTSV::TSV &t)
                {
                    covs[t.id] = t.kmer;
                    lens[t.id] = t.klen;
                });
            }

            Stats stats;

            return DAsssembly::analyze<C, Stats>(file, align, [&](C &node)
            {
                const auto id = Tokens::first(node.id, " ");

                if (covs.count(id))
                {
                    node.k_len = lens.at(id);
                    node.k_cov = covs.at(id);
                }
            });
        }
    };
}

#endif


#ifndef BART_SRC_CALCULATOR_RESIDUAL_DOMAIN_ISOTROPIC_RESIDUAL_I_HPP_
#define BART_SRC_CALCULATOR_RESIDUAL_DOMAIN_ISOTROPIC_RESIDUAL_I_HPP_

#include "utility/has_description.h"

#include <deal.II/lac/vector.h>

#include "system/moments/spherical_harmonic_i.h"

namespace bart::calculator::residual {

/*! \brief Interface for classes that calculate the isotropic scattering residual for a whole domain.
 *
 * In general, these classes will use a CellIsotropicResidualI class to calculate the residual for each cell \f$K\f$ and
 * combine them into a Vector that gives the total isotropic scattering residual at each global degree of freedom summed
 * over all groups,
 * \f[
 * \big< R \big> = \sum_{K}^{T_K}\sum_{g = 0}^{G - 1}R_{K,g, 0}
 * \f]
 */
class DomainIsotropicResidualI : public utility::HasDescription {
 public:
  using Vector = dealii::Vector<double>;
  using FluxMoments = system::moments::SphericalHarmonicI;
  virtual ~DomainIsotropicResidualI() = default;

  /*! \brief Calculate the domain isotropic scattering residual.
   *
   * @param current_scalar_flux_ the scalar flux for step \f$k + 1/2\f$
   * @param previous_scalar_flux_ the scalar flux for step \f$k\f$
   */
  virtual auto CalculateDomainResidual(FluxMoments* current_flux_moments,
                                       FluxMoments* previous_flux_moments) -> Vector = 0;
};

} // namespace bart::calculator::residual

#endif //BART_SRC_CALCULATOR_RESIDUAL_DOMAIN_ISOTROPIC_RESIDUAL_I_HPP_


/**Copyright (c) 2019 Hao Da (Kevin) Dong, Zuyang Cao, Jing Liang
 * @file       Xingyun.cpp
 * @date       10/13/2019
 * @brief      The file Xingyun.cpp implements a human perception class.
 *             The class will be used for detecting human from 2D Lidar data-sets.
 * @license    This project is released under the BSD-3-Clause License.
 */
#include <matplotlibcpp.h>
#include <math.h>
#include <iostream>
#include <vector>
#include <iterator>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <string>
#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/range/irange.hpp>

#include <Obstacle.hpp>
#include <Human.hpp>
#include <Xingyun.hpp>

namespace plt = matplotlibcpp;

#define LIDAR_RANGE 3.9
#define CLUSTER_THRESHOLD 0.3
#define LEG_DIAMETER 0.4
#define LEG_DISTANCE 0.3
#define MAJOR_AXIS 0.5
#define MINOR_AXIS 0.2
#define GRAD_DIFF_THRESHOLD 3

/** @brief Read data and classify the points into obstacles. */
void Xingyun::obstacleClassification() {
    double preDistance = rawLidarDistances[0];
    std::vector<double> xList, yList, distanceList;
    std::vector<std::vector<double>> objectBufferX, objectBufferY;
    std::vector<std::vector<std::vector<double>>> objects;

    // point cloud classification
    for (auto const& tupleValue : boost::combine(rawLidarDistances, pointCloudCartesian[0], pointCloudCartesian[1])) {
        double distance, x, y;
        boost::tie(distance, x, y) = tupleValue;

        if (fabs(distance - preDistance) >= CLUSTER_THRESHOLD) {
            if (distanceList.size() > 0) {
                if (distanceList[0] < LIDAR_RANGE) {
                    objectBufferX.push_back(xList);
                    objectBufferY.push_back(yList);
                }
                xList.clear();
                yList.clear();
                distanceList.clear();
            }
        }
        preDistance = distance;
        xList.push_back(x);
        yList.push_back(y);
        distanceList.push_back(distance);
    }
    if (distanceList.size() > 0) {
        if (distanceList[0] < LIDAR_RANGE) {
            objectBufferX.push_back(xList);
            objectBufferY.push_back(yList);
        }
    }

    // extract information needed for obstacle object
    for (auto const& tupleValue : boost::combine(objectBufferX, objectBufferY)) {
        std::vector<double> xTempList, yTempList, cartesianVector;
        Obstacle obstacleValue;

        boost::tie(xTempList, yTempList) = tupleValue;
        obstacleValue.rightMostPoint = { *(xTempList.end() - 1) , *(yTempList.end() - 1)};
        obstacleValue.leftMostPoint =  { *xTempList.begin() , *yTempList.begin() };
        obstacleValue.midPoint = {(*xTempList.begin() + *(xTempList.end() - 1))/2,
            (*(yTempList.end() - 1) + *yTempList.begin())/2 };

        std::vector<double> gradiences;
        auto const tupleCombination = boost::combine(xTempList, yTempList);
        for (auto beginIndex = tupleCombination.begin(); beginIndex < tupleCombination.end() - 1; beginIndex++) {
            double x, y, nextX, nextY;
            boost::tie(x, y) = *beginIndex;
            boost::tie(nextX, nextY) = *(beginIndex+1);

            double gradience;
            if ((nextX-x) == 0)
                gradience = 9999;
            else
                gradience = (nextY - y) / (nextX - x);
            if (gradience > 9999) gradience = 9999;
            gradiences.push_back(gradience);
        }

        obstacleValue.largestGrad = *std::max_element(gradiences.begin(), gradiences.end());
        obstacleValue.smallestGrad = *std::min_element(gradiences.begin(), gradiences.end());

        obstacleList.push_back(obstacleValue);
    }

    std::cout << "Number of obstacles: " << obstacleList.size() << std::endl;

    return;
}

/** @brief Recognize legs among obstacles. */
void Xingyun::legRecognition() {
    double obstacleLength = 0;   // Length of obstacle contour

    // Check each obstacle in obstacleList
    for (auto const obstacle : obstacleList) {
        // Check gradient constraint
        if (obstacle.largestGrad - obstacle.smallestGrad > GRAD_DIFF_THRESHOLD) {
            // Calculate length of obstacle contour
            obstacleLength = sqrt(pow(obstacle.rightMostPoint[0] - obstacle.leftMostPoint[0], 2)
                                    + pow(obstacle.rightMostPoint[1] - obstacle.leftMostPoint[1], 2));
            // Check diameter constraint. Add obstacle to legList if it passes both constraints.
            if (obstacleLength < LEG_DIAMETER)
                legList.push_back(obstacle);
        }
    }

    std::cout << "Number of legs: " << legList.size() << std::endl;

    return;
}

/** @brief Supporting function to humanRecognition() to process humans where both legs are visible
 *  @param queue 2-element inspection queue for leg pairs
*/
void Xingyun::processNormalHuman(std::vector<Obstacle> queue) {
    // Calculate human centroid from midpoints of legs
    std::vector<double> centroid;
    centroid.push_back((queue[0].midPoint[0] + queue[1].midPoint[0]) / 2);
    centroid.push_back((queue[0].midPoint[1] + queue[1].midPoint[1]) / 2);

    // Calculate orientation angle
    double orientationAngle = atan((queue[0].midPoint[1] - queue[1].midPoint[1]) / (queue[0].midPoint[0] - queue[1].midPoint[0]));

    // Create Human object and add it to humanList
    Human human;
    human.centroid = centroid;
    human.orientationAngle = orientationAngle + M_PI/2;
    humanList.push_back(human);
    return;
}

/** @brief Supporting function to humanRecognition() to process humans where only one leg is visible
 *  @param queue 2-element inspection queue for leg pairs
*/
void Xingyun::processSidewaysHuman(std::vector<Obstacle> queue) {
    // Calculate orientation angle directly from leg coordinates
    double orientation = atan(queue[0].midPoint[1] / queue[0].midPoint[0]);

    // Approximate human centroid from midpoints of legs
    std::vector<double> centroid;
    centroid.push_back(queue[0].midPoint[0] + (LEG_DISTANCE/2)*cos(orientation));
    centroid.push_back(queue[0].midPoint[1] + (LEG_DISTANCE/2)*sin(orientation));

    // Create Human object and add it to humanList
    Human human;
    human.centroid = centroid;
    human.orientationAngle = orientation + M_PI/2;
    humanList.push_back(human);
    return;
}

/** @brief Recognize humans from legs. */
void Xingyun::humanRecognition() {
    std::vector<Obstacle> queue, legListTemp;    // 2-element inspection queue for leg pairs
    legListTemp = legList;
    while (legListTemp.empty() == false) {
        // Pop first element of legList into the inspeaction queue
        queue.push_back(legListTemp.front());

        legListTemp.erase(legListTemp.begin());
        // Load up queue to two elements, unless legList is empty, in which case process the 1-element queue as a sideways human
        if (queue.size() < 2) {
            if (legListTemp.empty() == false) {
                continue;
            } else {
                processSidewaysHuman(queue);
                queue.pop_back();
                continue;
            }
        }

        // Calculate distance between legs in queue
        double distance = sqrt(pow(queue[0].midPoint[0] - queue[1].midPoint[0], 2)
                            + pow(queue[0].midPoint[1] - queue[1].midPoint[1], 2));

        // If the legs are close together, process the pair as a normal human then empty the queue
        if (distance < LEG_DISTANCE) {
            processNormalHuman(queue);
            queue.pop_back();
            queue.pop_back();
        } else {  // If legs are far apart, process the first leg as a sideways human and pop it out of the queue
            // Create temporary queue to hold the first leg
            std::vector<Obstacle> temp;
            temp.push_back(queue.front());
            queue.erase(queue.begin());
            processSidewaysHuman(temp);
        }
    }

    if (queue.empty() == false) {
        std::vector<Obstacle> temp;
        temp.push_back(queue.front());
        queue.erase(queue.begin());
        processSidewaysHuman(temp);
    }
}

/**
 * @brief Main function to detect human.
 * @param lidarDatasetFilename - Name of the data-set.
 * @return humanList - The detected human list.
 */
std::vector<Human> Xingyun::humanPerception(std::string lidarDatasetFilename) {
    // load lidar data from csv file to vector rawLidarDistances
    std::ifstream fileStream(lidarDatasetFilename);
    std::string item;
    while (std::getline(fileStream, item, ',')) {
        double value;
        std::stringstream readString;
        readString << item;
        readString >> value;
        rawLidarDistances.push_back(value);
    }
    if (rawLidarDistances.size() != 512) {
        std::cout << "didn't read file right: " << rawLidarDistances.size() << std::endl;
    }
    // get polar data from raw lidar data
    std::vector<double> angles;  // vector with 100 ints.
    for (int i : boost::irange(0, 512)) {angles.push_back((-120 + i * 240 / 512) * M_PI / 180);}

    pointCloudPolar.push_back(rawLidarDistances);
    pointCloudPolar.push_back(angles);

    // converte polar coordinates to cartesian coordinates
    std::vector<double> xValues, yValues;

    for (auto const& tupleValue : boost::combine(rawLidarDistances, angles)) {
        double distance, angle;
        boost::tie(distance, angle) = tupleValue;
        xValues.push_back(distance * cos(angle));
        yValues.push_back(distance * sin(angle));
    }
    pointCloudCartesian.push_back(xValues);
    pointCloudCartesian.push_back(yValues);

    obstacleClassification();
    legRecognition();
    humanRecognition();

    return humanList;
}

/** @brief Show the output map. */
void Xingyun::visualization() {
    plt::plot({ 0 }, { 0 }, "bs");  // Show robot as square at origin.
    for (auto human : humanList) {
           plt::plot({ human.centroid[0] }, { human.centroid[1] }, "ro");  // Plot human centroid in map.
           double orientation = human.orientationAngle;  // Human orientation radians, x-axis (pointing to the right) is 0 radians
           int n = 500;
           std::vector<double> x(n), y(n);
           for (int i = 0; i < n; ++i) {
               double t = 2 * M_PI * i / n;
               x.at(i) = MINOR_AXIS * cos(t) * cos(orientation)
                       - MAJOR_AXIS * sin(t) * sin(orientation) + human.centroid[0];
               y.at(i) = MINOR_AXIS * cos(t) * sin(orientation)
                       + MAJOR_AXIS * sin(t) * cos(orientation) + human.centroid[1];
           }
           plt::plot(x, y, "r-");  // Plot human as ellipse.
    }
    plt::xlim(-4, 4);
    plt::ylim(-4, 4);
    plt::show();
    return;
}

/** @brief Get rawLidarDistances.
 *  @return rawLidarDistances - Raw data.
 */
std::vector<double> Xingyun::getRawLidarDistances() {
    return rawLidarDistances;
}

/** @brief Get pointCloudPolar.
 *  @return pointCloudPolar - Converted data in Polar coordinate.
 */
std::vector<std::vector<double>> Xingyun::getPointCloudPolar() {
    return pointCloudPolar;
}

/** @brief Get pointCloudCartesian.
 *  @return pointCloudCartesian - Converted data in Cartesian coordinate.
 */
std::vector<std::vector<double>> Xingyun::getPointCloudCartesian() {
    return pointCloudCartesian;
}

/** @brief Get obstacleList.
 *  @return obstacleList - List of obstacles.
 */
std::vector<Obstacle> Xingyun::getObstacleList() {
    return obstacleList;
}

/** @brief Get legList.
 *  @return legList - List of legs.
 */
std::vector<Obstacle> Xingyun::getLegList() {
    return legList;
}

/** @brief Get humanList.
 *  @return humanList - List of humans.
 */
std::vector<Human> Xingyun::getHumanList() {
    return humanList;
}


#include <iostream>
#include <type_traits>

#include <boost/msm/front/euml/euml.hpp>
#include <boost/msm/front/euml/state_grammar.hpp>
#include <boost/msm/back/state_machine.hpp>

using namespace boost::msm::front::euml;

BOOST_MSM_EUML_DECLARE_ATTRIBUTE(int, switched_on)

BOOST_MSM_EUML_STATE((), Off)
BOOST_MSM_EUML_STATE((), On)

BOOST_MSM_EUML_EVENT(press)

BOOST_MSM_EUML_ACTION(WriteMessage)
{
    template<class Evt, class Fsm, class SourceState, class TargetState>
    void operator()(const Evt&, Fsm&, const SourceState&, const TargetState&)
    {
        if constexpr (std::is_same_v<SourceState, decltype(Off)>)
            std::cout << "Switched on\n";
        else
            std::cout << "Switched off\n";
    }
};

BOOST_MSM_EUML_TRANSITION_TABLE((
    Off + press [fsm_(switched_on) < Int_<2>()] / (++fsm_(switched_on), WriteMessage()) == On,
    On + press / WriteMessage() == Off
), light_transition_table)

BOOST_MSM_EUML_DECLARE_STATE_MACHINE((
    light_transition_table, init_ << Off, no_action, no_action, attributes_ << switched_on
), light_state_machine)

int main()
{
    boost::msm::back::state_machine<light_state_machine> light;
    std::cout << "Current state: " << *(light.current_state()) << "\n";
    light.process_event(press);
    std::cout << "Current state: " << *(light.current_state()) << "\n";
    light.process_event(press);
    std::cout << "Current state: " << *(light.current_state()) << "\n";
    light.process_event(press);
    std::cout << "Current state: " << *(light.current_state()) << "\n";
    light.process_event(press);
    std::cout << "Current state: " << *(light.current_state()) << "\n";
    light.process_event(press);
    std::cout << "Current state: " << *(light.current_state()) << "\n";
}


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

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/if_zero_else.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( ifnot_minus_
                            , (typename A0, typename A1, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::fundamental_<A0>, X>
                            , bs::pack_<bd::unspecified_<A1>, X>
                            , bs::pack_<bd::unspecified_<A1>, X>
                            )
  {
    BOOST_FORCEINLINE A1 operator()(A0 const& a0, A1 const& a1, A1 const& a2) const BOOST_NOEXCEPT
    {
      return a1-if_zero_else(a0, a2);
    }
  };
} } }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP

#include <boost/range.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/convert.hpp>

#include <boost/geometry/algorithms/detail/overlay/append_no_duplicates.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

/*!
    \brief Strategy: line clipping algorithm after Liang Barsky
    \ingroup overlay
    \details The Liang-Barsky line clipping algorithm clips a line with a clipping box.
    It is slightly adapted in the sense that it returns which points are clipped
    \tparam B input box type of clipping box
    \tparam P input/output point-type of segments to be clipped
    \note The algorithm is currently only implemented for 2D Cartesian points
    \note Though it is implemented in namespace strategy, and theoretically another
        strategy could be used, it is not (yet) updated to the general strategy concepts,
        and not (yet) splitted into a file in folder strategies
    \author Barend Gehrels, and the following recourses
    - A tutorial: http://www.skytopia.com/project/articles/compsci/clipping.html
    - a German applet (link broken): http://ls7-www.cs.uni-dortmund.de/students/projectgroups/acit/lineclip.shtml
*/
template<typename Box, typename Point>
class liang_barsky
{
private:
    typedef model::referring_segment<Point> segment_type;

    template <typename T>
    inline bool check_edge(T const& p, T const& q, T& t1, T& t2) const
    {
        bool visible = true;

        if(p < 0)
        {
            T const r = q / p;
            if (r > t2)
                visible = false;
            else if (r > t1)
                t1 = r;
        }
        else if(p > 0)
        {
            T const r = q / p;
            if (r < t1)
                visible = false;
            else if (r < t2)
                t2 = r;
        }
        else
        {
            if (q < 0)
                visible = false;
        }

        return visible;
    }

public:

    inline bool clip_segment(Box const& b, segment_type& s, bool& sp1_clipped, bool& sp2_clipped) const
    {
        typedef typename select_coordinate_type<Box, Point>::type coordinate_type;

        coordinate_type t1 = 0;
        coordinate_type t2 = 1;

        coordinate_type const dx = get<1, 0>(s) - get<0, 0>(s);
        coordinate_type const dy = get<1, 1>(s) - get<0, 1>(s);

        coordinate_type const p1 = -dx;
        coordinate_type const p2 = dx;
        coordinate_type const p3 = -dy;
        coordinate_type const p4 = dy;

        coordinate_type const q1 = get<0, 0>(s) - get<min_corner, 0>(b);
        coordinate_type const q2 = get<max_corner, 0>(b) - get<0, 0>(s);
        coordinate_type const q3 = get<0, 1>(s) - get<min_corner, 1>(b);
        coordinate_type const q4 = get<max_corner, 1>(b) - get<0, 1>(s);

        if (check_edge(p1, q1, t1, t2)      // left
            && check_edge(p2, q2, t1, t2)   // right
            && check_edge(p3, q3, t1, t2)   // bottom
            && check_edge(p4, q4, t1, t2))   // top
        {
            sp1_clipped = t1 > 0;
            sp2_clipped = t2 < 1;

            if (sp2_clipped)
            {
                set<1, 0>(s, get<0, 0>(s) + t2 * dx);
                set<1, 1>(s, get<0, 1>(s) + t2 * dy);
            }

            if(sp1_clipped)
            {
                set<0, 0>(s, get<0, 0>(s) + t1 * dx);
                set<0, 1>(s, get<0, 1>(s) + t1 * dy);
            }

            return true;
        }

        return false;
    }

    template<typename Linestring, typename OutputIterator>
    inline void apply(Linestring& line_out, OutputIterator out) const
    {
        if (!boost::empty(line_out))
        {
            *out = line_out;
            ++out;
            geometry::clear(line_out);
        }
    }
};

}} // namespace strategy::intersection

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

/*!
    \brief Clips a linestring with a box
    \details A linestring is intersected (clipped) by the specified box
    and the resulting linestring, or pieces of linestrings, are sent to the specified output operator.
    \tparam OutputLinestring type of the output linestrings
    \tparam OutputIterator an output iterator which outputs linestrings
    \tparam Linestring linestring-type, for example a vector of points, matching the output-iterator type,
         the points should also match the input-iterator type
    \tparam Box box type
    \tparam Strategy strategy, a clipping strategy which should implement the methods "clip_segment" and "apply"
*/
template
<
    typename OutputLinestring,
    typename OutputIterator,
    typename Range,
    typename Box,
    typename Strategy
>
OutputIterator clip_range_with_box(Box const& b, Range const& range,
            OutputIterator out, Strategy const& strategy)
{
    if (boost::begin(range) == boost::end(range))
    {
        return out;
    }

    typedef typename point_type<OutputLinestring>::type point_type;

    OutputLinestring line_out;

    typedef typename boost::range_iterator<Range const>::type iterator_type;
    iterator_type vertex = boost::begin(range);
    for(iterator_type previous = vertex++;
            vertex != boost::end(range);
            ++previous, ++vertex)
    {
        point_type p1, p2;
        geometry::convert(*previous, p1);
        geometry::convert(*vertex, p2);

        // Clip the segment. Five situations:
        // 1. Segment is invisible, finish line if any (shouldn't occur)
        // 2. Segment is completely visible. Add (p1)-p2 to line
        // 3. Point 1 is invisible (clipped), point 2 is visible. Start new line from p1-p2...
        // 4. Point 1 is visible, point 2 is invisible (clipped). End the line with ...p2
        // 5. Point 1 and point 2 are both invisible (clipped). Start/finish an independant line p1-p2
        //
        // This results in:
        // a. if p1 is clipped, start new line
        // b. if segment is partly or completely visible, add the segment
        // c. if p2 is clipped, end the line

        bool c1 = false;
        bool c2 = false;
        model::referring_segment<point_type> s(p1, p2);

        if (!strategy.clip_segment(b, s, c1, c2))
        {
            strategy.apply(line_out, out);
        }
        else
        {
            // a. If necessary, finish the line and add a start a new one
            if (c1)
            {
                strategy.apply(line_out, out);
            }

            // b. Add p1 only if it is the first point, then add p2
            if (boost::empty(line_out))
            {
                detail::overlay::append_no_duplicates(line_out, p1, true);
            }
            detail::overlay::append_no_duplicates(line_out, p2);

            // c. If c2 is clipped, finish the line
            if (c2)
            {
                strategy.apply(line_out, out);
            }
        }

    }

    // Add last part
    strategy.apply(line_out, out);
    return out;
}

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_CLIP_LINESTRING_HPP