#include <iostream>
#include <functional>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "mg_navigation.hpp"
#include "handlers/dwa.hpp"
namespace mg {

    template <typename T>
    rclcpp_action::GoalResponse MgNavigationServer::handle_goal(const rclcpp_action::GoalUUID& /*unused*/,
                                                                typename T::Goal::ConstSharedPtr /*unused*/,
                                                                const char* N) {
        RCLCPP_INFO(get_logger(), "Recieved goal for action: [%s]", N);
        const std::lock_guard<std::mutex> lock(mtx);
        if (is_processing) {
            return rclcpp_action::GoalResponse::REJECT;
        } else {
            is_processing = true;
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
        }
    }

    template <typename T>
    rclcpp_action::CancelResponse MgNavigationServer::handle_cancel(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char* N) {
        RCLCPP_INFO(this->get_logger(), "Recieved cancel request for action: [%s]", N);
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    template <typename T>
    void MgNavigationServer::handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh,
                                             const char*                                                N) {
        RCLCPP_INFO(this->get_logger(), "Goal accepted for action: [%s]", N);
        using namespace std::placeholders;
        auto t = std::bind(&MgNavigationServer::execute<T>, this, _1);
        std::thread{ t, gh }.detach();
    }

    template <typename T>
    void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
        DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
        dwm.execute();
        is_processing = false;
        mtx.unlock();
    }

    MgNavigationServer::MgNavigationServer(rclcpp::NodeOptions& opts) : rclcpp::Node("MgNavigationServer", opts) {
        tf2_buffer   = std::make_shared<tf2_ros::Buffer>(get_clock());
        tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);

        path_buffer_ = std::make_shared<PathBuffer>(this);

        pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);

        obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);

        using namespace std::placeholders;

        sv_move_global = rclcpp_action::create_server<MoveGlobal>(
          this,
          "MoveGlobal",
          std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
          std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
          std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));

        sv_move_point = rclcpp_action::create_server<MovePoint>(
          this,
          "MovePoint",
          std::bind(&MgNavigationServer::handle_goal<MovePoint>, this, _1, _2, "MovePoint"),
          std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
          std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));

        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
          this,
          "MoveStraight",
          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));

        sv_look_at = rclcpp_action::create_server<LookAt>(
          this,
          "LookAt",
          std::bind(&MgNavigationServer::handle_goal<LookAt>, this, _1, _2, "LookAt"),
          std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
          std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));

        sv_rotate = rclcpp_action::create_server<Rotate>(
          this,
          "Rotate",
          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
    }

};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions opts;
    rclcpp::spin(std::make_shared<mg::MgNavigationServer>(opts));
    rclcpp::shutdown();
    return 0;
}