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

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

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

TcpClient::TcpClient() {
    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 tcp client message >>>> " << 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. " << 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") {
        _slam.control_robotic_arm();
    } else if (received_string == "i") {
        _slam.move_to_i();
    } else if (received_string == "o") {
        _slam.move_to_o();
    } 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;

            //连接成功后把自己的编号发送给Tcp Service
            const std::string dog_code = std::string("devCode:") + ConstantConfig::DOG_CODE;
            send(_client_socket, dog_code.c_str(), dog_code.size(), 0);

            // 启动接收数据的线程
            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));
        }
    }
}