//
// Created by casic on 25-2-25.
//

#ifndef SLAM_WRAPPER_HPP
#define SLAM_WRAPPER_HPP

#include <vector>

#include "unitree/common/dds/dds_easy_model.hpp"
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>

#define COMMANDTOPIC "rt/qt_command"
#define NOTICETOPIC "rt/qt_notice"
#define ODOMTOPIC "rt/lio_sam_ros2/mapping/re_location_odometry"
#define ADDNODETOPIC "rt/qt_add_node"
#define ADDEDGETOPIC "rt/qt_add_edge"

using namespace unitree::robot;

struct node_attribute {
    u_int16_t nodeName;
    float nodeX;
    float nodeY;
    float nodeZ;
    float nodeYaw;
};

struct edge_attribute {
    u_int16_t edgeName;
    u_int16_t edgeStart;
    u_int16_t edgeEnd;
};

class slam_wrapper {
private:
    int index = 0;
    // const nav_msgs::msg::dds_::Odometry_ *currentOdom{};

    // pub
    // ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtCommand_> pubQtCommand = ChannelPublisherPtr<
    //     unitree_interfaces::msg::dds_::QtCommand_>(
    //     new ChannelPublisher<unitree_interfaces::msg::dds_::QtCommand_>(COMMANDTOPIC)
    // );
    // ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtNode_> pubQtNode = ChannelPublisherPtr<
    //     unitree_interfaces::msg::dds_::QtNode_>(
    //     new ChannelPublisher<unitree_interfaces::msg::dds_::QtNode_>(ADDNODETOPIC)
    // );
    // ChannelPublisherPtr<unitree_interfaces::msg::dds_::QtEdge_> pubQtEdge = ChannelPublisherPtr<
    //     unitree_interfaces::msg::dds_::QtEdge_>(
    //     new ChannelPublisher<unitree_interfaces::msg::dds_::QtEdge_>(ADDEDGETOPIC)
    // );

    // sub
    // ChannelSubscriberPtr<std_msgs::msg::dds_::String_> subQtNotice = ChannelSubscriberPtr<
    //     std_msgs::msg::dds_::String_>(
    //     new ChannelSubscriber<std_msgs::msg::dds_::String_>(NOTICETOPIC)
    // );
    // ChannelSubscriberPtr<nav_msgs::msg::dds_::Odometry_> subOdometer = ChannelSubscriberPtr<
    //     nav_msgs::msg::dds_::Odometry_>(
    //     new ChannelSubscriber<nav_msgs::msg::dds_::Odometry_>(ODOMTOPIC)
    // );

public:
    std::vector<node_attribute> node_attributes;
    std::vector<edge_attribute> edge_attributes;
    u_int16_t node_name = 0;

    explicit slam_wrapper(const char *networkInterface);

    void qt_notice_handler(const void *message);

    void odometer_handler(const void *message);

    void start_mapping();

    void end_mapping();

    void start_relocation();

    void init_pose();

    void start_navigation();

    void default_navigation();

    void add_node_and_edge();

    void add_edge(u_int16_t edge_name, u_int16_t start_node, u_int16_t end_node);

    void save_node_and_edge();

    void close_all_node();

    void delete_all_node();

    void delete_all_edge();

    void pause_navigation();

    void recover_navigation();
};

#endif //SLAM_WRAPPER_HPP