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

#include "slam_wrapper.hpp"
#include <cmath>

slam_wrapper::slam_wrapper(const char *networkInterface) {
    ChannelFactory::Instance()->Init(0, networkInterface);

    // pub
    pubQtCommand->InitChannel();
    pubQtNode->InitChannel();
    pubQtEdge->InitChannel();

    // sub
    subQtNotice->InitChannel(std::bind(&slam_wrapper::qt_notice_handler, this, std::placeholders::_1), 10);
    subOdometer->InitChannel(std::bind(&slam_wrapper::odometer_handler, this, std::placeholders::_1), 1);
}

void slam_wrapper::qt_notice_handler(const void *message) {
    int index_, begin_, end_, feedback_, arrive_;
    const std_msgs::msg::dds_::String_ *seq = (const std_msgs::msg::dds_::String_ *) message;
    std::string str_, notice_;

    begin_ = seq->data().find("index:", 0); // Instruction unique identifier
    end_ = seq->data().find(";", begin_);
    str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6);
    index_ = atoi(str_.c_str());

    begin_ = seq->data().find("notice:", 0); // Prompt message
    end_ = seq->data().find(";", begin_);
    notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);

    if (index_ <= 10000) {
        // Command execution feedback
        begin_ = seq->data().find("feedback:", 0);
        end_ = seq->data().find(";", begin_);
        str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9);
        feedback_ = atoi(str_.c_str());
    } else if (index_ == 10001) {
        // Navigation feedback
        begin_ = seq->data().find("arrive:", 0);
        end_ = seq->data().find(";", begin_);
        str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7);
        arrive_ = atoi(str_.c_str());
    }
}

void slam_wrapper::odometer_handler(const void *message) {
    currentOdom = (const nav_msgs::msg::dds_::Odometry_ *) message;
}

void slam_wrapper::start_mapping() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 3; // 3 is to start mapping command
    send_msg.attribute_() = 1;
    //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::end_mapping() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.command_() = 4; // 4 is the end mapping command
    send_msg.attribute_() = 0;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.floor_index_().push_back(0); // Floor number，just give a fixed value of 0.
    send_msg.pcdmap_index_().push_back(0); // PCD Map number，just give a fixed value of 0.
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::start_relocation() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 6; // 6 is to start relocation command
    send_msg.attribute_() = 1;
    //When this value is 1, the XT16 LiDAR node is enabled, and when it is 2, the MID360 LiDAR node is enabled. Please confirm the value based on the actual LiDAR being
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::init_pose() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 7; // 7 is the pose initialization instruction
    send_msg.quaternion_x_() = 0; // Quaternion
    send_msg.quaternion_y_() = 0;
    send_msg.quaternion_z_() = 0;
    send_msg.quaternion_w_() = 1;
    send_msg.translation_x_() = 0; // Translation
    send_msg.translation_y_() = 0;
    send_msg.translation_z_() = 0;
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::start_navigation() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 8; // 8 is the command to start navigation
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::default_navigation() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 10; // 10 Multi nodes loop navigation(default) command
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::add_node_and_edge() {
    node_attribute nodeTemp{};
    float siny_cosp, cosy_cosp;

    siny_cosp = 2 * (currentOdom->pose().pose().orientation().w() * currentOdom->pose().pose().orientation().z() +
                     currentOdom->pose().pose().orientation().x() * currentOdom->pose().pose().orientation().y());
    cosy_cosp = 1 - 2 * (currentOdom->pose().pose().orientation().y() * currentOdom->pose().pose().orientation().y() +
                         currentOdom->pose().pose().orientation().z() * currentOdom->pose().pose().orientation().z());
    node_name++;

    nodeTemp.nodeX = currentOdom->pose().pose().position().x();
    nodeTemp.nodeY = currentOdom->pose().pose().position().y();
    nodeTemp.nodeZ = currentOdom->pose().pose().position().z();
    nodeTemp.nodeYaw = std::atan2(siny_cosp, cosy_cosp);
    nodeTemp.nodeName = node_name;

    node_attributes.push_back(nodeTemp);

    if (node_name >= 2) {
        add_edge(node_name - 1, node_name - 1, node_name); // Sequential connection nodes
    }
}

void slam_wrapper::add_edge(u_int16_t edge_name, u_int16_t start_node, u_int16_t end_node) {
    edge_attribute edgeTemp{};

    edgeTemp.edgeName = edge_name;
    edgeTemp.edgeStart = start_node;
    edgeTemp.edgeEnd = end_node;
    edge_attributes.push_back(edgeTemp);
}

void slam_wrapper::save_node_and_edge() {
    unitree_interfaces::msg::dds_::QtNode_ nodeMsg;
    unitree_interfaces::msg::dds_::QtEdge_ edgeMsg;
    if (edge_attributes.empty()) {
        return;
    }

    index++;
    nodeMsg.seq_().data() = "index:" + std::to_string(index) + ";";
    for (auto &[nodeName, nodeX, nodeY, nodeZ, nodeYaw]: node_attributes) {
        nodeMsg.node_().node_name_().push_back(nodeName); // Node name
        nodeMsg.node_().node_position_x_().push_back(nodeX); // Point X coordinate information
        nodeMsg.node_().node_position_y_().push_back(nodeY); // Point Y coordinate information
        nodeMsg.node_().node_position_z_().push_back(nodeZ); // Point Z coordinate information
        nodeMsg.node_().node_yaw_().push_back(nodeYaw); // Point Yaw Angle Information
        nodeMsg.node_().node_attribute_().push_back(0);
        // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!!
        nodeMsg.node_().undefined_().push_back(0);
        nodeMsg.node_().node_state_2_().push_back(0);
        nodeMsg.node_().node_state_3_().push_back(0);
    }
    pubQtNode->Write(nodeMsg);

    index++;
    edgeMsg.seq_().data() = "index:" + std::to_string(index) + ";";
    for (auto &[edgeName, edgeStart, edgeEnd]: edge_attributes) {
        edgeMsg.edge_().edge_name_().push_back(edgeName); // Edge name
        edgeMsg.edge_().start_node_name_().push_back(edgeStart); // Start node name
        edgeMsg.edge_().end_node_name_().push_back(edgeEnd); // End node name
        edgeMsg.edge_().edge_length_().push_back(0);
        edgeMsg.edge_().dog_speed_().push_back(1); // The speed of the dog walking this edge (0-1)
        edgeMsg.edge_().edge_state_2_().push_back(0);
        // 0：Stop 1：Avoid 3：Replan  when encountering obstacles. Suggest assigning a value of 0.

        edgeMsg.edge_().dog_stats_().push_back(0);
        // Not open attribute, please assign a value of 0, note: cannot be empty!!! Cannot be empty!!! Cannot be empty!!!
        edgeMsg.edge_().dog_back_stats_().push_back(0);
        edgeMsg.edge_().edge_state_().push_back(0);
        edgeMsg.edge_().edge_state_1_().push_back(0);
        edgeMsg.edge_().edge_state_3_().push_back(0);
        edgeMsg.edge_().edge_state_4_().push_back(0);
    }
    pubQtEdge->Write(edgeMsg);
}

void slam_wrapper::close_all_node() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 99; // 99 Close all nodes command
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::delete_all_node() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 1; // 1 is delete instruction
    send_msg.attribute_() = 1; // 1 is select delete node
    send_msg.node_edge_name_().push_back(999);
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::delete_all_edge() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 1; // 1 is delete instruction
    send_msg.attribute_() = 2; // 2 is select delete edge
    send_msg.node_edge_name_().push_back(999);
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::pause_navigation() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 13; // 13 is the pause navigation command
    pubQtCommand->Write(send_msg);
}

void slam_wrapper::recover_navigation() {
    index++;
    unitree_interfaces::msg::dds_::QtCommand_ send_msg;
    send_msg.seq_().data() = "index:" + std::to_string(index) + ";";
    send_msg.command_() = 14; // 14 to recover navigation commands
    pubQtCommand->Write(send_msg);
}