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

#include "slam_wrapper.hpp"
#include "tcp_service.hpp"
#include "constant_config.hpp"

#include <cmath>
#include <thread>
#include <fstream>
#include <iostream>
#include <boost/beast/core.hpp>
#include <boost/beast/websocket.hpp>
#include <boost/asio/connect.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/beast/version.hpp>
#include <boost/beast/http.hpp>

namespace http = boost::beast::http;
using tcp = boost::asio::ip::tcp;

static std::string trim(const std::string &s) {
    auto start = s.begin();
    while (start != s.end() && std::isspace(*start)) ++start;

    auto end = s.end();
    do {
        --end;
    } while (std::distance(start, end) > 0 && std::isspace(*end));

    return std::string(start, end + 1);
}

static std::string generate_multipart_form(const std::string &file_path, const std::string &boundary,
                                           std::string &content_type) {
    std::ifstream file(file_path, std::ios::binary);
    if (!file) {
        throw std::runtime_error("无法打开文件: " + file_path);
    }

    // 提取文件名（避免暴露完整路径）
    std::string file_name = file_path.substr(file_path.find_last_of("/\\") + 1);

    // 读取文件内容
    std::ostringstream content_stream;
    content_stream << file.rdbuf();
    std::string file_content = content_stream.str();

    // 构建 multipart/form-data 内容
    std::ostringstream body;
    body << "--" << boundary << "\r\n";
    body << R"(Content-Disposition: form-data; name="file"; filename=")" << file_name << "\"\r\n";
    body << "Content-Type: application/octet-stream\r\n\r\n";
    body << file_content;
    body << "\r\n--" << boundary << "--\r\n";

    content_type = "multipart/form-data; boundary=" + boundary;
    return body.str();
}

static void upload_pcd_file() {
    std::cout << "upload_pcd_file start" << std::endl;
    try {
        // 设置 Boost.Asio 上下文
        boost::asio::io_context ioc;
        tcp::resolver resolver(ioc);
        tcp::socket socket(ioc);

        // 解析并连接服务器
        auto const results = resolver.resolve(ConstantConfig::SERVICE_ADDRESS, ConstantConfig::HTTP_PORT);
        boost::asio::connect(socket, results.begin(), results.end());

        // 构建 HTTP 请求
        http::request<http::string_body> req{http::verb::post, ConstantConfig::API_UPLOAD_PCD_FILE, 11};
        req.set(http::field::host, ConstantConfig::SERVICE_ADDRESS);
        req.set(http::field::user_agent, BOOST_BEAST_VERSION_STRING);

        // 构建 multipart/form-data 数据
        std::string boundary = "---------------------------boundary_example_" + std::to_string(std::time(nullptr));
        std::string contentType;
        std::string body = generate_multipart_form(ConstantConfig::SLAM_PCD_FILE, boundary, contentType);
        if (body.empty()) {
            throw std::runtime_error("Failed to generate multipart form data");
        }
        req.body() = body;
        req.prepare_payload();
        req.set(http::field::content_type, contentType);
        http::write(socket, req);

        // 接收响应
        boost::beast::flat_buffer buffer;
        http::response<http::dynamic_body> res;
        http::read(socket, buffer, res);
        std::cout << "状态码: " << res.result_int() << "\n";

        // 清空响应体以避免残留数据
        ostream(res.body()) << "";

        // 关闭 socket
        boost::system::error_code ec;
        socket.shutdown(tcp::socket::shutdown_both, ec);
        if (ec) {
            std::cerr << "Socket shutdown error: " << ec.message() << std::endl;
        }
    } catch (std::exception const &e) {
        std::cerr << "错误: " << e.what() << std::endl;
    }
    std::cout << "upload_pcd_file end" << std::endl;
}

SlamWrapper::SlamWrapper() {
    ChannelFactory::Instance()->Init(0, ConstantConfig::SLAM_NET_PORT);

    std::thread serial_port_thread([this] {
        //初始化机械臂串口
        boost::asio::io_service io;
        serial_port_ = new RoboticArmSerialPort(io, ConstantConfig::ROBOTIC_ARM_PORT, ConstantConfig::BAUD_RATE);
        io.run();
    });
    serial_port_thread.detach();

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

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

    // 读取本地指令配置
    std::ifstream config_file(ConstantConfig::ROBOTIC_ARM_COMMAND_CONFIG);
    if (!config_file.is_open()) {
        std::cerr << "Failed to open config file" << std::endl;
        return;
    }

    std::string line;
    std::string current_section;
    CommandAttribute current_command;
    while (std::getline(config_file, line)) {
        line = trim(line);
        if (line.empty() || line[0] == '#') continue; // 忽略空行和注释

        if (line[0] == '[') {
            // 处理节名 [section]
            size_t end_bracket = line.find(']');
            if (end_bracket != std::string::npos) {
                if (!current_section.empty()) {
                    // 将上一个节的数据存储到 map 中
                    arm_commands_[current_section + ".command"] = current_command;
                }
                current_section = line.substr(1, end_bracket - 1);
                // 初始化新的节数据
                current_command = {};
            }
        } else {
            size_t delimiter_pos = line.find('=');
            if (delimiter_pos != std::string::npos) {
                std::string key = line.substr(0, delimiter_pos);
                std::string value = line.substr(delimiter_pos + 1);

                if (key == "command") {
                    // 将字符串形式的 hex 指令转为 uint8_t vector
                    std::istringstream ss(value);
                    std::string byte_str;
                    std::vector<uint8_t> bytes;

                    while (ss >> byte_str) {
                        bytes.push_back(static_cast<uint8_t>(std::strtoul(byte_str.c_str(), nullptr, 16)));
                    }

                    current_command.command = bytes;
                } else if (key == "enabled") {
                    // 解析启用状态
                    current_command.enabled = value == "1";
                }
            }
        }
    }
    // 存储最后一个节的数据
    if (!current_section.empty()) {
        arm_commands_[current_section + ".command"] = current_command;
    }
    config_file.close();

    std::cout << "SlamWrapper init success, has " << arm_commands_.size() << " arm commands " << std::endl;
}

//casic start
void SlamWrapper::control_robotic_arm() {
    const std::vector<uint8_t> left_command = {
        0XFE, 0XFE, 0X0F, 0X3C, 0X05, 0XB3, 0X08, 0X19, 0X07, 0X5E, 0X08, 0X73, 0X08, 0X1D, 0X08, 0XA6, 0X14, 0XFA
    };
    const std::vector<uint8_t> right_command = {
        0XFE, 0XFE, 0X0F, 0X3C, 0X0A, 0X98, 0X07, 0X6F, 0X06, 0X91, 0X07, 0XCD, 0X0A, 0X93, 0X08, 0X56, 0X14, 0XFA
    };

    std::cout << "control_robotic_arm start" << std::endl;
    serial_port_->write(left_command);
    std::this_thread::sleep_for(std::chrono::seconds(5));
    serial_port_->write(right_command);
    std::this_thread::sleep_for(std::chrono::seconds(5));
    serial_port_->write(default_command);
    std::cout << "control_robotic_arm end" << std::endl;
}

void SlamWrapper::move_to_i() {
    const std::vector<uint8_t> left_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA
    };
    const std::vector<uint8_t> right_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0xB1, 0x08, 0x6B, 0x07, 0x57, 0x08, 0x4D, 0x0B, 0x89, 0x07, 0xE2, 0x1E, 0xFA
    };

    std::cout << "move_to_i start" << std::endl;
    serial_port_->write(left_command);
    std::this_thread::sleep_for(std::chrono::seconds(6));
    serial_port_->write(right_command);
    std::this_thread::sleep_for(std::chrono::seconds(9));
    serial_port_->write(default_command);
    std::cout << "move_to_i end" << std::endl;
}

void SlamWrapper::move_to_o() {
    const std::vector<uint8_t> left_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA
    };
    const std::vector<uint8_t> right_command = {
        0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x4D, 0x08, 0xD5, 0x07, 0x5D, 0x08, 0x94, 0x0A, 0xD5, 0x07, 0xFA, 0x05, 0xFA
    };

    std::cout << "move_to_o start" << std::endl;
    serial_port_->write(left_command);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    serial_port_->write(right_command);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    serial_port_->write(default_command);
    std::cout << "move_to_o end" << std::endl;
}

std::vector<uint8_t> SlamWrapper::get_command(const int index) {
    const std::string section = std::to_string(index);
    const std::string full_key = section + ".command";

    const auto it = arm_commands_.find(full_key);
    if (it != arm_commands_.end() && it->second.enabled) {
        return it->second.command;
    }
    return {};
}

//casic end

void SlamWrapper::qt_notice_handler(const void *message) {
    const auto seq = static_cast<const String_ *>(message);
    std::string str_;

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

    begin_ = seq->data().find("notice:", 0); // Prompt message
    end_ = seq->data().find(";", begin_);
    const std::string 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);
        if (const int feedback_ = atoi(str_.c_str()); feedback_ == 0 || feedback_ == -1) {
            std::cout << "\033[1;31m"
                    << "Command execution failed with index = " << index_ << "."
                    << "\033[0m";
        }
        std::cout << notice_ << std::endl;
    } 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);
        const int arrive_ = atoi(str_.c_str());
        std::cout << "I arrived node " << arrive_ << ". " << notice_ << std::endl;
        const std::vector<uint8_t> command = get_command(arrive_);
        if (command.empty()) {
            std::cout << "No command found for node " << arrive_ << ". " << notice_ << std::endl;
            return;
        }
        //发送暂停指令
        pause_navigation();
        //倒计时结束后恢复巡检
        std::thread countdown_thread([this,&arrive_, command] {
            try {
                std::cout << "发送机械臂转动指令，倒计时开始，请等待10秒" << std::endl;
                serial_port_->write(command);
                std::this_thread::sleep_for(std::chrono::seconds(5));

                //延迟5~10秒更新节点
                TcpService::getInstance().update_node(arrive_);

                std::this_thread::sleep_for(std::chrono::seconds(5));

                recover_navigation();
                std::cout << "倒计时结束，恢复机械臂初始位置，恢复巡检";
                serial_port_->write(default_command);
            } catch (const std::exception &e) {
                std::cerr << "线程执行过程中发生异常: " << e.what() << std::endl;
            }
        });
        countdown_thread.join();
    }
}

void SlamWrapper::odometer_handler(const void *message) {
    currentOdom = static_cast<const Odometry_ *>(message);
}

void SlamWrapper::start_mapping() {
    index++;
    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 SlamWrapper::end_mapping() {
    index++; //
    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);
    upload_pcd_file();
}

void SlamWrapper::start_relocation() {
    index++;
    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 SlamWrapper::init_pose() {
    index++;
    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 SlamWrapper::start_navigation() {
    index++;
    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 SlamWrapper::default_navigation() {
    index++;
    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 SlamWrapper::add_node_and_edge() {
    NodeAttribute nodeTemp{};

    const float siny_cosp = 2 * (currentOdom->pose().pose().orientation().w()
                                 * currentOdom->pose().pose().orientation().z()
                                 + currentOdom->pose().pose().orientation().x()
                                 * currentOdom->pose().pose().orientation().y());
    const float 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 SlamWrapper::add_edge(const u_int16_t edge_name, const u_int16_t start_node, u_int16_t end_node) {
    EdgeAttribute edgeTemp{};

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

void SlamWrapper::save_node_and_edge() {
    QtNode_ nodeMsg;
    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 SlamWrapper::close_all_node() {
    index++;
    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 SlamWrapper::delete_all_node() {
    index++;
    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 SlamWrapper::delete_all_edge() {
    index++;
    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 SlamWrapper::pause_navigation() {
    std::cout << "Pause navigation start" << std::endl;
    index++;
    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);
    std::cout << "Pause navigation end" << std::endl;
}

void SlamWrapper::recover_navigation() {
    index++;
    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);
}