#ifndef __NAVIGATION_HPP__
#define __NAVIGATION_HPP__

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

#include <unitree/idl/ros2/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 poseDate
{
public:
    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;
    float q_x = 0.0f;
    float q_y = 0.0f;
    float q_z = 0.0f;
    float q_w = 1.0f;
    int mode = 1;
    float speed = 0.8f;
    std::string toJsonStr() const
    {
        nlohmann::json j;
        j["data"]["targetPose"]["x"] = x;
        j["data"]["targetPose"]["y"] = y;
        j["data"]["targetPose"]["z"] = z;
        j["data"]["targetPose"]["q_x"] = q_x;
        j["data"]["targetPose"]["q_y"] = q_y;
        j["data"]["targetPose"]["q_z"] = q_z;
        j["data"]["targetPose"]["q_w"] = q_w;
        j["data"]["mode"] = mode;
        j["data"]["speed"] = speed;
        return j.dump(4);
    }
    void printInfo() const
    {
        std::cout << "x:" << x << " y:" << y << " z:" << z << " q_x:"
                  << q_x << " q_y:" << q_y << " q_z:" << q_z << " q_w:" << q_w << std::endl;
    }
};

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();

    poseDate 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);

    unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_> subSlamInfo;
    unitree::robot::ChannelSubscriberPtr<std_msgs::msg::dds_::String_> subSlamKeyInfo;

    poseDate currentPose;
    bool is_arrived = false;
};

} // namespace custom

#endif // __NAVIGATION_HPP__