/** Copyright 2025 The Magrob Authors
 *
 * 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.
 */
#pragma once

#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "mg_msgs/action/move_point.hpp"
#include "mg_msgs/action/move_straight.hpp"
#include "mg_msgs/action/look_at.hpp"

#include "geometry_msgs/msg/twist_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

namespace mg {

    namespace Geometry = geometry_msgs::msg;

    class MgNavigationServer : public rclcpp::Node {
      public:
        RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
        using MovePoint    = mg_msgs::action::MovePoint;
        using MoveStraight = mg_msgs::action::MoveStraight;
        using LookAt       = mg_msgs::action::LookAt;

        rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;

        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
        rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
        rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;

        rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
        tf2_ros::Buffer::SharedPtr                                  tf2_buffer;
        std::shared_ptr<tf2_ros::TransformListener>                 tf2_listener;
        std::mutex                                                  mtx;

        MgNavigationServer(rclcpp::NodeOptions& opts);

      private:
        bool is_processing = false;

        template <typename T>
        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
                                                typename T::Goal::ConstSharedPtr,
                                                const char*);
        template <typename T>
        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >,
                                                    const char*);
        template <typename T>
        void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char*);

        template <typename T>
        void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >);
    };

};