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

#ifndef SLAM_WRAPPER_HPP
#define SLAM_WRAPPER_HPP

#include <vector>

#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>

#include <unitree/ros2_idl/QtCommand_.hpp>
#include <unitree/ros2_idl/String_.hpp>
#include <unitree/ros2_idl/Odometry_.hpp>
#include <unitree/ros2_idl/QtNode_.hpp>
#include <unitree/ros2_idl/QtEdge_.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"

#include "tcp_service.hpp"

using namespace unitree::robot;
using namespace unitree_interfaces::msg::dds_;
using namespace std_msgs::msg::dds_;
using namespace nav_msgs::msg::dds_;

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

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

class SlamWrapper {
public:
    std::vector<NodeAttribute> node_attributes;
    std::vector<EdgeAttribute> edge_attributes;
    u_int16_t node_name = 0;

    void register_observer(const std::shared_ptr<MessageObserver> &observer);

    void notify_observers(const std::vector<uint8_t> &command);

    void notify_observers(const int &node);

    void control_robotic_arm();

    explicit SlamWrapper();

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

private:
    int index = 0;
    const Odometry_ *currentOdom{};
    std::vector<std::shared_ptr<MessageObserver> > observers;
    const std::vector<uint8_t> init_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x14, 0xFA
    };

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

    // sub
    ChannelSubscriberPtr<String_> subQtNotice = ChannelSubscriberPtr<String_>(
        new ChannelSubscriber<String_>(NOTICETOPIC)
    );
    ChannelSubscriberPtr<Odometry_> subOdometer = ChannelSubscriberPtr<Odometry_>(
        new ChannelSubscriber<Odometry_>(ODOMTOPIC)
    );
};

#endif //SLAM_WRAPPER_HPP