//
// 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 "robotic_arm_serial_port.hpp"
#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;
};

struct CommandAttribute {
    std::vector<uint8_t> command;
    bool enabled;
};

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

    explicit SlamWrapper();

    //casic start
    void control_robotic_arm();

    void move_to_i();

    void move_to_o();

    std::vector<uint8_t> get_command(int index);
    //casic end

    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{};
    RoboticArmSerialPort *serial_port_;
    const std::vector<uint8_t> default_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x08, 0x20, 0x09, 0x90, 0x05, 0x96, 0x08, 0x26, 0x0A, 0xFE, 0x08, 0x48, 0x1E, 0xFA
    };
    std::map<std::string, CommandAttribute> arm_commands_;

    // 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