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

#include "tcp_client.hpp"
#include "tcp_service.hpp"
#include "slam_wrapper.hpp"

#include <cstring>
#include <iostream>
#include <arpa/inet.h>
#include <stdexcept>
#include <thread>

TcpClient::TcpClient() {
    _slam = new SlamWrapper();
    // 启动TCP服务，接收AI结果并通过串口控制机械臂
    std::thread service_thread([this] {
        boost::asio::io_service io;
        const auto service = std::make_shared<TcpService>(io, "/dev/ttyACM0", 115200); // 机械臂串口
        _slam->register_observer(service);
        service->start(8888);
        io.run();
    });
    service_thread.detach();
    std::cout << "TcpClient::init success" << std::endl;
}

void TcpClient::handle_data(const std::vector<char> &buffer) {
    const std::string received_string(buffer.begin(), buffer.end());
    std::cout << "handle_data >>>> " << received_string << std::endl;
    if (received_string == "q") {
        std::cout << "Close all nodes" << std::endl;
        _slam->close_all_node();
    } else if (received_string == "w") {
        std::cout << "Start mapping (default to clearing node/edge information)" << std::endl;
        _slam->delete_all_node();
        _slam->delete_all_edge();
        _slam->node_attributes.clear();
        _slam->edge_attributes.clear();
        _slam->start_mapping();
        _slam->node_name = 0;
    } else if (received_string == "e") {
        std::cout << "End mapping" << std::endl;
        _slam->end_mapping();
    } else if (received_string == "a") {
        std::cout << "Start navigation (default)" << std::endl;
        _slam->start_relocation();
        _slam->start_navigation();
        _slam->init_pose();
        _slam->default_navigation();
    } else if (received_string == "s") {
        _slam->pause_navigation();
    } else if (received_string == "d") {
        std::cout << "Recover navigation" << std::endl;
        _slam->recover_navigation();
    } else if (received_string == "z") {
        std::cout << "Start relocation and navigation to prepare for collecting node/edge information. "
                << "(Default clearing of node/edge information)"
                << std::endl;
        _slam->delete_all_node();
        _slam->delete_all_edge();
        _slam->start_relocation();
        _slam->start_navigation();
        _slam->init_pose();
    } else if (received_string == "x") {
        std::cout << "Collect node/edge information" << std::endl;
        _slam->add_node_and_edge();
    } else if (received_string == "c") {
        std::cout << "Save node/edge information" << std::endl;
        _slam->save_node_and_edge();
        _slam->node_attributes.clear();
        _slam->edge_attributes.clear();
        _slam->node_name = 0;
    } else if (received_string == "v") {
        std::cout << "Clear node/edge information" << std::endl;
        _slam->delete_all_node();
        _slam->delete_all_edge();
        _slam->node_name = 0;
    } else if (received_string == "scan") {
        std::cout << "Control robotic arm start" << std::endl;
        _slam->control_robotic_arm();
    } else {
        std::cout << "Unknown command" << std::endl;
    }
}

void TcpClient::receive_data() {
    std::string received_data;
    received_data.reserve(8192);
    while (true) {
        //检查 _should_stop 状态时候，使用锁来保证线程安全
        {
            std::lock_guard lock(_mutex);
            if (_should_stop) {
                break;
            }
        }

        constexpr size_t buffer_size = 64;
        char temp_buffer[buffer_size];
        const ssize_t bytes_received = recv(_client_socket, temp_buffer, buffer_size, 0);
        if (bytes_received < 0) {
            std::cerr << "failed to receive data: " << strerror(errno) << std::endl;
            break;
        }

        if (bytes_received == 0) {
            std::cerr << "connection closed by server" << std::endl;
            break;
        }

        received_data.append(temp_buffer, bytes_received);

        // 查找结束标志
        while (true) {
            const size_t pos = received_data.find('/');
            if (pos == std::string::npos) {
                break;
            }

            std::string_view data_packet(received_data.data(), pos);
            handle_data(std::vector(data_packet.begin(), data_packet.end()));

            received_data.erase(0, pos + 1); // 移除已处理的数据包和结束标志
        }
    }
}

void TcpClient::connect(const std::string &server_ip, const int server_port) {
    if (server_ip.empty() || server_port <= 0 || server_port > 65535) {
        throw std::invalid_argument("invalid server ip or port");
    }

    for (int attempt = 0; attempt < _max_retries; ++attempt) {
        try {
            _client_socket = socket(AF_INET, SOCK_STREAM, 0);
            if (_client_socket == -1) {
                throw std::runtime_error("failed to create socket");
            }

            sockaddr_in server_addr{};
            server_addr.sin_family = AF_INET;
            server_addr.sin_port = htons(server_port);
            inet_pton(AF_INET, server_ip.c_str(), &server_addr.sin_addr);

            if (::connect(_client_socket, reinterpret_cast<sockaddr *>(&server_addr), sizeof(server_addr)) == -1) {
                close(_client_socket);
                _client_socket = -1;
                std::this_thread::sleep_for(std::chrono::milliseconds(_retry_interval));
                continue;
            }

            std::cout << "TcpClient::connected to " << server_ip << ":" << server_port
                    << ", waiting for command ..." << std::endl;

            // 启动接收数据的线程
            std::thread receive_thread(&TcpClient::receive_data, this);

            // 等待接收线程结束
            receive_thread.join();
        } catch (const std::exception &e) {
            if (attempt == _max_retries - 1) {
                std::cout << "TcpClient::connect failed: " << e.what() << std::endl;
                throw;
            }
            std::cout << "TcpClient::connect failed! "
                    << "Attempting to connect (" << attempt + 1 << "/" << _max_retries << ")..." << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(_retry_interval));
        }
    }
}