#include "hsm/details/fill_dispatch_table.h"
#include "hsm/details/index_map.h"
#include "hsm/details/state.h"
#include "hsm/details/transition_table.h"

#include "hsm/front/transition_tuple.h"

#include <gtest/gtest.h>
#include <boost/hana.hpp>

using namespace ::testing;

namespace bh {
    using namespace boost::hana;
}

namespace {

class S1 {
};
class S2 {
};

    struct P {
        static constexpr auto make_transition_table()
        {
            return hsm::transition_table(
                hsm::transition(hsm::state_t<S2> {}, 0, 0, 0, hsm::state_t<S2> {}));
        }
    };

    struct S {
        static constexpr auto make_transition_table()
        {
            return hsm::transition_table(
                hsm::transition(hsm::state_t<S1> {}, 0, 0, 0, hsm::state_t<P> {}));
        }
    };

    constexpr auto getCombinedStateTypeids()
    {
        return hsm::getCombinedStateTypeids(hsm::state_t<S> {});
    }
}

class StateidxTests : public Test {
};

TEST_F(StateidxTests, should_get_stateidx)
{
    EXPECT_EQ(bh::size_c<3>, hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<S> {}));
    EXPECT_EQ(bh::size_c<0>, hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<S1> {}));
    EXPECT_EQ(bh::size_c<1>, hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<P> {}));
    EXPECT_EQ(bh::size_c<2>, hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<S2> {}));
}

TEST_F(StateidxTests, should_get_parent_stateidx)
{
    EXPECT_EQ(bh::size_c<0>, hsm::getParentStateIdx(hsm::state_t<S> {}, hsm::state_t<S> {}));
    EXPECT_EQ(bh::size_c<1>, hsm::getParentStateIdx(hsm::state_t<S> {}, hsm::state_t<P> {}));
}

TEST_F(StateidxTests, should_get_combined_stateidx)
{
    EXPECT_EQ(
        bh::size_c<0>,
        hsm::getCombinedStateIdx(
            getCombinedStateTypeids(), hsm::state_t<S> {}, hsm::state_t<S1> {}));
    EXPECT_EQ(
        bh::size_c<6>,
        hsm::getCombinedStateIdx(
            getCombinedStateTypeids(), hsm::state_t<P> {}, hsm::state_t<S2> {}));
}

TEST_F(StateidxTests, should_calculate_combined_stateidx)
{
    EXPECT_EQ(bh::size_c<4>, hsm::nStates(hsm::state_t<S> {}));
    EXPECT_EQ(bh::size_c<2>, hsm::nParentStates(hsm::state_t<S> {}));

    auto s1StateIdx = hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<S1> {});
    auto s2StateIdx = hsm::getStateIdx(hsm::state_t<S> {}, hsm::state_t<S2> {});
    auto sParentStateIdx = hsm::getParentStateIdx(hsm::state_t<S> {}, hsm::state_t<S> {});
    auto pParentStateIdx = hsm::getParentStateIdx(hsm::state_t<S> {}, hsm::state_t<P> {});

    EXPECT_EQ(
        hsm::calcCombinedStateIdx(hsm::nStates(hsm::state_t<S> {}), sParentStateIdx, s1StateIdx),
        hsm::getCombinedStateIdx(
            getCombinedStateTypeids(), hsm::state_t<S> {}, hsm::state_t<S1> {}));
    EXPECT_EQ(
        hsm::calcCombinedStateIdx(hsm::nStates(hsm::state_t<S> {}), pParentStateIdx, s2StateIdx),
        hsm::getCombinedStateIdx(
            getCombinedStateTypeids(), hsm::state_t<P> {}, hsm::state_t<S2> {}));
}

TEST_F(StateidxTests, should_calculate_parent_stateidx)
{
    auto s1CombinedStateIdx = hsm::getCombinedStateIdx(
        getCombinedStateTypeids(), hsm::state_t<S> {}, hsm::state_t<S1> {});
    auto s2CombinedStateIdx = hsm::getCombinedStateIdx(
        getCombinedStateTypeids(), hsm::state_t<P> {}, hsm::state_t<S2> {});

    EXPECT_EQ(0, hsm::calcParentStateIdx(hsm::nStates(hsm::state_t<S> {}), s1CombinedStateIdx));
    EXPECT_EQ(1, hsm::calcParentStateIdx(hsm::nStates(hsm::state_t<S> {}), s2CombinedStateIdx));
}

TEST_F(StateidxTests, should_calculate_stateidx)
{
    auto s1CombinedStateIdx = hsm::getCombinedStateIdx(
        getCombinedStateTypeids(), hsm::state_t<S> {}, hsm::state_t<S1> {});
    auto s2CombinedStateIdx = hsm::getCombinedStateIdx(
        getCombinedStateTypeids(), hsm::state_t<P> {}, hsm::state_t<S2> {});

    EXPECT_EQ(0, hsm::calcStateIdx(hsm::nStates(hsm::state_t<S> {}), s1CombinedStateIdx));
    EXPECT_EQ(2, hsm::calcStateIdx(hsm::nStates(hsm::state_t<S> {}), s2CombinedStateIdx));
}


#include <boost/mpl/arithmetic.hpp>


#define BOOST_TEST_MODULE "test_read_hard_core_excluded_volume_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/input/read_global_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_hard_core_excluded_volume_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_hard_core_excluded_volume.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            epsilon                 = 3.14
            cutoff                  = 2.71
            ignore.molecule         = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            parameters  = [
                {index = 0, core_radius = 2.0, soft_shell_thickness = 3.0},
                {index = 1, core_radius = 2.0, soft_shell_thickness = 2.0},
                {index = 3, core_radius = 3.0, soft_shell_thickness = 100.0},
            ]
        )"_toml;

        const auto pot_para = mjolnir::read_hard_core_excluded_volume_potential<traits_type>(v);
        const auto& pot  = pot_para.first;
        const auto& para = dynamic_cast<mjolnir::HardCoreExcludedVolumeParameterList<traits_type> const&>(pot_para.second.cref());

        const auto ignore_within = para.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(para.participants().size() == 3u);
        BOOST_TEST(para.participants().at(0)  == 0u);
        BOOST_TEST(para.participants().at(1)  == 1u);
        BOOST_TEST(para.participants().at(2)  == 3u);

        // soft_shell_thickness (sigma)
        BOOST_TEST(para.parameters().at(0).sigma == real_type(  3.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(1).sigma == real_type(  2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(3).sigma == real_type(100.0), tolerance<real_type>());

        // core_radius (r0)
        BOOST_TEST(para.parameters().at(0).radius == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(1).radius == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(3).radius == real_type(3.0), tolerance<real_type>());

        BOOST_TEST(pot.epsilon()      == real_type(3.14), tolerance<real_type>());
        BOOST_TEST(pot.cutoff_ratio() == real_type(2.71), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_hard_core_excluded_volume_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_hard_core_excluded_volume.log");

    using real_type = T;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            epsilon                 = 3.14
            ignore.molecule         = "Nothing"
            ignore.particles_within.bond    = 3
            ignore.particles_within.contact = 1
            env.two      = 2.0
            env.three    = 3.0
            env.toolarge = 100.0
            parameters  = [
                {index =   0, core_radius =   "two", soft_shell_thickness = 3.0},
                {index =   1, core_radius =     2.0, soft_shell_thickness = "two"},
                {index =   3, core_radius = "three", soft_shell_thickness = "toolarge"},
            ]
        )"_toml;

        const auto pot_para = mjolnir::read_hard_core_excluded_volume_potential<traits_type>(v);
        const auto& pot  = pot_para.first;
        const auto& para = dynamic_cast<mjolnir::HardCoreExcludedVolumeParameterList<traits_type> const&>(pot_para.second.cref());

        const auto ignore_within = para.exclusion_list().ignore_topology();
        const std::map<std::string, std::size_t> within(
                ignore_within.begin(), ignore_within.end());

        BOOST_TEST(within.size() == 2u);
        BOOST_TEST(within.at("bond")    == 3ul);
        BOOST_TEST(within.at("contact") == 1ul);

        BOOST_TEST(para.participants().size() == 3u);
        BOOST_TEST(para.participants().at(0)  == 0u);
        BOOST_TEST(para.participants().at(1)  == 1u);
        BOOST_TEST(para.participants().at(2)  == 3u);

        // soft_shell_thickness (sigma)
        BOOST_TEST(para.parameters().at(0).sigma == real_type(  3.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(1).sigma == real_type(  2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(3).sigma == real_type(100.0), tolerance<real_type>());

        // core radius (r0)
        BOOST_TEST(para.parameters().at(0).radius == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(1).radius == real_type(2.0), tolerance<real_type>());
        BOOST_TEST(para.parameters().at(3).radius == real_type(3.0), tolerance<real_type>());

        BOOST_TEST(pot.epsilon()      == real_type(3.14), tolerance<real_type>());
    }
}


#include <boost/test/unit_test.hpp>
#include <templ/symbols/Value.hpp>
#include <templ/symbols/values/Int.hpp>
#include <templ/solvers/temporal/Event.hpp>
#include <templ/solvers/temporal/PersistenceCondition.hpp>
#include <templ/solvers/temporal/Timeline.hpp>

using namespace templ;
using namespace templ::symbols;
using namespace templ::solvers::temporal;

namespace pa = templ::solvers::temporal::point_algebra;

BOOST_AUTO_TEST_SUITE(timeline)

BOOST_AUTO_TEST_CASE(persistence_conditions)
{
    StateVariable stateVariable("robot_location","robot0");
    Timeline timeline(stateVariable);

    // rloc(robot0)@[t0,t1) : l0
    ObjectVariable::Ptr l0(new ObjectVariable("l0"));
    ObjectVariable::Ptr l1(new ObjectVariable("l1"));
    ObjectVariable::Ptr l2(new ObjectVariable("l2"));

    point_algebra::TimePoint::Ptr t0(new point_algebra::QualitativeTimePoint("t0"));
    point_algebra::TimePoint::Ptr t1(new point_algebra::QualitativeTimePoint("t1"));
    point_algebra::TimePoint::Ptr t2(new point_algebra::QualitativeTimePoint("t2"));
    point_algebra::TimePoint::Ptr t3(new point_algebra::QualitativeTimePoint("t3"));

    PersistenceCondition::Ptr assertion0(new PersistenceCondition(stateVariable, l0, t0, t1));
    timeline.addTemporalAssertion(assertion0);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with 1 assertion is consistent");

    PersistenceCondition::Ptr assertion1(new PersistenceCondition(stateVariable, l0, t0, t1));
    timeline.addTemporalAssertion(assertion1);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with same assertions consistent");

    {
        // rloc(robot0)@[t2,t3) : l1
        PersistenceCondition::Ptr assertion1(new PersistenceCondition(stateVariable, l1, t2, t3));
        timeline.addTemporalAssertion(assertion1);
        BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with 1 assertion is consistent");
    }

    {
        // rloc(robot0)@[t2,t3) : l3
        PersistenceCondition::Ptr assertion1(new PersistenceCondition(stateVariable, l2, t2, t3));
        timeline.addTemporalAssertion(assertion1);
        BOOST_REQUIRE_MESSAGE(!timeline.isConsistent(), "Timeline with 1 assertion is inconsistent");
    }
}

BOOST_AUTO_TEST_CASE(events)
{
    StateVariable stateVariable("robot_location","robot0");
    Timeline timeline(stateVariable);

    // rloc(robot0)@[t0,t1) : l0
    ObjectVariable::Ptr l0 = ObjectVariable::getInstance("l0");
    ObjectVariable::Ptr l1 = ObjectVariable::getInstance("l1");
    ObjectVariable::Ptr l2 = ObjectVariable::getInstance("l2");

    point_algebra::TimePoint::Ptr t0 = point_algebra::QualitativeTimePoint::getInstance("t0");
    point_algebra::TimePoint::Ptr t1 = point_algebra::QualitativeTimePoint::getInstance("t1");
    point_algebra::TimePoint::Ptr t2 = point_algebra::QualitativeTimePoint::getInstance("t2");
    point_algebra::TimePoint::Ptr t3 = point_algebra::QualitativeTimePoint::getInstance("t3");

    Event::Ptr assertion0 = Event::getInstance(stateVariable, l0, l1, t0);
    Event::Ptr assertion1 = Event::getInstance(stateVariable, l1, l2, t1);
    Event::Ptr assertion2 = Event::getInstance(stateVariable, l2, l2, t2);
    Event::Ptr assertion3 = Event::getInstance(stateVariable, l1, l2, t2);

    timeline.addTemporalAssertion(assertion0);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with 1 assertion is consistent");
    timeline.addTemporalAssertion(assertion0);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with same assertions consistent");
    timeline.addTemporalAssertion(assertion1);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with 3 valid assertions consistent");
    timeline.addTemporalAssertion(assertion2);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with 4 valid assertions consistent");

    timeline.addTemporalAssertion(assertion3);
    BOOST_REQUIRE_MESSAGE(!timeline.isConsistent(), "Timeline with invalid assertions is not consistent");
}

BOOST_AUTO_TEST_CASE(persistence_conditions_and_events)
{
    StateVariable stateVariable("robot_location","robot0");
    Timeline timeline(stateVariable);

    // rloc(robot0)@[t0,t1) : l0
    ObjectVariable::Ptr l0 = ObjectVariable::getInstance("l0");
    ObjectVariable::Ptr l1 = ObjectVariable::getInstance("l1");
    ObjectVariable::Ptr l2 = ObjectVariable::getInstance("l2");

    point_algebra::TimePoint::Ptr t0 = point_algebra::QualitativeTimePoint::getInstance("t0");
    point_algebra::TimePoint::Ptr t1 = point_algebra::QualitativeTimePoint::getInstance("t1");
    point_algebra::TimePoint::Ptr t2 = point_algebra::QualitativeTimePoint::getInstance("t2");
    point_algebra::TimePoint::Ptr t3 = point_algebra::QualitativeTimePoint::getInstance("t3");

    Event::Ptr event0 = Event::getInstance(stateVariable, l0, l1, t0);
    Event::Ptr event1 = Event::getInstance(stateVariable, l1, l2, t1);
    Event::Ptr event2 = Event::getInstance(stateVariable, l2, l2, t2);
    Event::Ptr event3 = Event::getInstance(stateVariable, l1, l2, t2);

    PersistenceCondition::Ptr pc0 = PersistenceCondition::getInstance(stateVariable, l1, t0, t1);
    PersistenceCondition::Ptr pc1 = PersistenceCondition::getInstance(stateVariable, l2, t1, t2);

    timeline.addTemporalAssertion(event0);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with one event consistent");
    timeline.addTemporalAssertion(pc0);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with one event and one persistence condition consistent");
    timeline.addTemporalAssertion(event1);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with two events and one persistence condition consistent");
    timeline.addTemporalAssertion(pc1);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with two events and two persistence conditions consistent");
    timeline.addTemporalAssertion(event2);
    BOOST_REQUIRE_MESSAGE(timeline.isConsistent(), "Timeline with three events and two persistence conditions is consistent");
    timeline.addTemporalAssertion(event3);
    BOOST_REQUIRE_MESSAGE(!timeline.isConsistent(), "Timeline with four events and two persistence conditions is inconsistent");
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/euler/include/functions/gamma.hpp>
#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/timing.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <cmath>

using nt2::tag::gamma_;

#define RS(T,V1,V2) (T, T(V1) ,T(V2))

namespace n1 {
  typedef float T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  NT2_TIMING(gamma_,(RS(T,T(-33),T(36))))
}
namespace n2 {
  typedef double T;
  typedef boost::dispatch::meta::as_integer<T>::type iT;
  NT2_TIMING(gamma_,(RS(T,T(-33),T(36))))
}
namespace n3 {
  typedef float T;
  NT2_TIMING(gamma_,(RS(T,T(-36),T(-33))))
}
namespace n4 {
  typedef double T;
  NT2_TIMING(gamma_,(RS(T,T(-36),T(-33))))
}
#undef RS


#include <boost/thread.hpp>
#include <iostream>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <math.h>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <c2_algorithm.h>
#include <velocity_smoother.h>

velocity_smoother::velocity_smoother()
{
    nh.param<double>("/velocity_smoother_node/v_max_vel", v_max_vel, 0.3);
    nh.param<double>("/velocity_smoother_node/v_min_vel", v_min_vel, 0.0);
    nh.param<double>("/velocity_smoother_node/v_max_acc", v_max_acc, 0.0);
    nh.param<double>("/velocity_smoother_node/v_min_acc", v_min_acc, 0.0);
    nh.param<double>("/velocity_smoother_node/v_jeck", v_jeck, 0.0);

    nh.param<double>("/velocity_smoother_node/a_max_vel", a_max_vel, 0.3);
    nh.param<double>("/velocity_smoother_node/a_min_vel", a_min_vel, 0.0);
    nh.param<double>("/velocity_smoother_node/a_max_acc", a_max_acc, 0.0);
    nh.param<double>("/velocity_smoother_node/a_min_acc", a_min_acc, 0.0);
    nh.param<double>("/velocity_smoother_node/a_jeck", a_jeck, 0.0);

    nh.param<std::string>("/velocity_smoother_node/raw_cmd_vel_topic", raw_vel_topic, "");
    nh.param<std::string>("/velocity_smoother_node/smooth_cmd_vel_topic", smooth_vel_topic, ""); //feedback_topic
    nh.param<std::string>("/velocity_smoother_node/robot_feedback_topic", feedback_topic, "");

    cout << "smooth_vel_topic:" << smooth_vel_topic << endl;
    cout << "raw_vel_topic:" << raw_vel_topic << endl;
    cout << "feedback_topic:" << feedback_topic << endl;
    cout << "v_max_vel:" << v_max_vel << endl;
    cout << "v_min_vel:" << v_min_vel << endl;
    cout << "v_max_acc:" << v_max_acc << endl;
    cout << "v_min_acc:" << v_min_acc << endl;
    cout << "v_jeck:" << v_jeck << endl;

    v_last_pos = 0;
    v_last_vel = 0;
    v_last_acc = 0;

    a_last_acc = 0;
    a_last_pos = 0;
    a_last_vel = 0;
    vel_c2.init(v_min_vel, v_max_vel, v_min_acc, v_max_acc, v_jeck, 100);
    ang_c2.init(a_min_vel, a_max_vel, a_min_acc, a_max_acc, a_jeck, 100);

    cmd_pub = nh.advertise<geometry_msgs::Twist>(smooth_vel_topic, 1, true);
    vel_sub = nh.subscribe(raw_vel_topic, 1, &velocity_smoother::velocity_cb, this);
    if (!feedback_topic.empty())
    {
        ROS_INFO("node add subscribe feedback_topic");
        odom_sub = nh.subscribe(feedback_topic, 1, &velocity_smoother::odom_cb, this);
    }
}

velocity_smoother ::~velocity_smoother()
{
}

void velocity_smoother::velocity_cb(const geometry_msgs::Twist::ConstPtr &msg)
{
    ROS_DEBUG_STREAM("get raw x:" << v_t_vel << " z:" << a_t_vel << endl);
    //cout << "get raw x:" << v_t_vel << " z:" << a_t_vel << endl;
    if (is_use_odom >= 1)
    {
        mutex.lock();
        //set linear
        //v_vel = v_last_vel;
        v_t_vel = msg->linear.x;
        v_acc = v_last_acc;
        v_t_acc = 0;
        vel_c2.start(v_vel, v_t_vel, v_acc, v_t_acc);
        //set angular
        //a_vel = a_last_vel;
        a_t_vel = msg->angular.z;
        a_acc = a_last_acc;
        a_t_acc = 0;
        ang_c2.start(a_vel, a_t_vel, a_acc, a_t_acc);
        mutex.unlock();
    }
    else
    {
        mutex.lock();
        //set linear
        v_vel = v_last_vel;
        v_t_vel = msg->linear.x;
        v_acc = v_last_acc;
        v_t_acc = 0;
        vel_c2.start(v_vel, v_t_vel, v_acc, v_t_acc);
        //set angular
        a_vel = a_last_vel;
        a_t_vel = msg->angular.z;
        a_acc = a_last_acc;
        a_t_acc = 0;
        ang_c2.start(a_vel, a_t_vel, a_acc, a_t_acc);
        mutex.unlock();
    }
}

void velocity_smoother::odom_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    mutex.lock();
    v_last_vel = (msg->twist.linear.x) / 1000;
    a_last_vel = (msg->twist.angular.z)/1000;
    mutex.unlock();
}

void velocity_smoother::update()
{
    //calc twist
    mutex.lock();

    vel_c2.get_qk(v_last_acc, v_last_vel, v_acc, v_vel);
    v_last_pos = v_pos;
    //v_last_vel = v_vel;
    v_last_acc = v_acc;

    ang_c2.get_qk(a_last_acc, a_last_vel, a_acc, a_vel);
    a_last_pos = a_pos;
    //a_last_vel = a_vel;
    a_last_acc = a_acc;
    if (feedback_topic.empty()){
        v_last_vel = v_vel;
        a_last_vel = a_vel;
    }
    mutex.unlock();

    geometry_msgs::Twist twist;
    twist.linear.x = v_vel;
    twist.angular.z = a_vel;
    cmd_pub.publish(twist);
    ROS_DEBUG_STREAM("deal smooth x:" << v_vel << " z:" << a_vel << endl);
}
void velocity_smoother::reset()
{
    v_last_vel = 0;
    v_last_acc = 0;
    a_last_vel = 0;
    a_last_acc = 0;
    v_t_vel = 0;
    v_t_acc = 0;
    a_t_vel = 0;
    a_t_acc = 0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "velocity_smoother_node");

    velocity_smoother vs;

    //set frequency
    ros::Rate rate(100);

    while (ros::ok())
    {
        vs.update();
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


#pragma once
#ifndef IDOCK_RECEPTOR_HPP
#define IDOCK_RECEPTOR_HPP

#include <boost/filesystem/path.hpp>
#include "atom.hpp"
#include "scoring_function.hpp"
using namespace boost::filesystem;

//! Represents a receptor.
class receptor
{
public:
	vector<atom> atoms; //!< Heavy atoms.
	const array<float, 3> center; //!< Box center.
	const array<float, 3> size; //!< 3D sizes of box.
	const array<float, 3> corner0; //!< Box boundary corner with smallest values of all the 3 dimensions.
	const array<float, 3> corner1; //!< Box boundary corner with largest values of all the 3 dimensions.
	const float granularity; //!< 1D size of grids.
	const float granularity_inverse; //!< 1 / granularity.
	const array<int, 3> num_probes; //!< Number of probes.
	const size_t num_probes_product; //!< Product of num_probes[0,1,2].
	const size_t map_bytes; //!< Number of bytes in a map.
	vector<vector<size_t>> p_offset; //!< Auxiliary precalculated constants to accelerate grid map creation.
	vector<vector<float>> maps; //!< Grid maps.

	//! Constructs a receptor by parsing a receptor file in PDBQT format.
	explicit receptor(const path& p, const array<float, 3>& center, const array<float, 3>& size, const float granularity);

	//! Precalculates auxiliary constants to accelerate grid map creation.
	void precalculate(const scoring_function& sf, const vector<size_t>& xs);

	//! Populates grid maps for certain atom types along X and Y dimensions for a given Z dimension value.
	void populate(const vector<size_t>& xs, const size_t z, const scoring_function& sf);
};

#endif