#ifndef __NAVIGATION_HPP__
#define __NAVIGATION_HPP__

#include <unitree/robot/client/client.hpp>
#include <nlohmann/json.hpp>
#include <string>
#include <iostream>

#include "std_msgs/msg/dds_/String_.hpp"
#include "unitree/robot/channel/channel_subscriber.hpp"


const std::string SLAM_SERVICE_NAME = "slam_operate";
const std::string SLAM_API_VERSION = "1.0.0";
const int32_t ROBOT_API_ID_STOP_NODE = 1901;
const int32_t ROBOT_API_ID_START_MAPPING_PL = 1801;
const int32_t ROBOT_API_ID_END_MAPPING_PL = 1802;
const int32_t ROBOT_API_ID_START_RELOCATION_PL = 1804;
const int32_t ROBOT_API_ID_POSE_NAV_PL = 1102;
const int32_t ROBOT_API_ID_PAUSE_NAV = 1201;
const int32_t ROBOT_API_ID_RESUME_NAV = 1202;

// DDS topic names
#define SlamInfoTopic "rt/slam_info"
#define SlamKeyInfoTopic "rt/slam_key_info"

namespace custom {

class Navigation : public unitree::robot::Client {
public:
    Navigation();
    ~Navigation();

    void Init() override;

    bool startMapping();
    bool endMapping(const std::string& address);
    bool initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address);
    bool poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed);
    bool pauseNavigation();
    bool resumeNavigation();
    bool closeSlam();

    PoseData getCurrentPose() const { return currentPose; }

    bool hasArrived() const { return is_arrived; }

private:
    bool callSlamService(int api_id, const nlohmann::json& data);
    void slamInfoHandler(const void *message);
    void slamKeyInfoHandler(const void *message);

    ChannelSubscriberPtr subSlamInfo;
    ChannelSubscriberPtr subSlamKeyInfo;

    // Structure to hold pose data
    struct PoseData {
        double x = 0.0;
        double y = 0.0;
        double z = 0.0;
        double q_x = 0.0;
        double q_y = 0.0;
        double q_z = 0.0;
        double q_w = 0.0;
    };

    PoseData currentPose;
    bool is_arrived = false;
};

} // namespace custom

#endif // __NAVIGATION_HPP__