// chassis_domain_server.cpp
// 底盘域 TCP 服务，监听端口 9001（默认）。
// 每个连接处理一帧后关闭（与 Ubuntu 侧 ChassisBridgeNode 行为一致）。

#include "chassis_domain_server.hpp"

#include <cstdio>
#include <cstring>
#include <thread>
#include <stdexcept>

#include "frame_codec.hpp"
#include "data_types.hpp"

namespace vaw
{

ChassisDomainServer::ChassisDomainServer(uint16_t port, ChassisHandler* handler)
    : port_(port), handler_(handler), running_(false), listen_sock_(SOCK_INVALID)
{}

ChassisDomainServer::~ChassisDomainServer()
{
    stop();
}

void ChassisDomainServer::run()
{
#ifdef _WIN32
    WSADATA wsa;
    WSAStartup(MAKEWORD(2, 2), &wsa);
#endif

    listen_sock_ = ::socket(AF_INET, SOCK_STREAM, 0);
    if (listen_sock_ == SOCK_INVALID)
        throw std::runtime_error("底盘：创建 socket 失败");

    int opt = 1;
    ::setsockopt(listen_sock_, SOL_SOCKET, SO_REUSEADDR,
                 reinterpret_cast<const char*>(&opt), sizeof(opt));

    sockaddr_in addr{};
    addr.sin_family      = AF_INET;
    addr.sin_port        = htons(port_);
    addr.sin_addr.s_addr = INADDR_ANY;

    if (::bind(listen_sock_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
        throw std::runtime_error("底盘：bind 失败");

    ::listen(listen_sock_, 8);
    running_ = true;
    std::printf("[chassis] 监听端口 %d\n", port_);

    while (running_) {
        sockaddr_in client_addr{};
#ifdef _WIN32
        int addr_len = sizeof(client_addr);
#else
        socklen_t addr_len = sizeof(client_addr);
#endif
        sock_t conn = ::accept(listen_sock_,
                               reinterpret_cast<sockaddr*>(&client_addr), &addr_len);
        if (conn == SOCK_INVALID) break;

        // 每个连接起一个线程处理，detach 后自动回收
        std::thread([this, conn]() { handle_connection(conn); }).detach();
    }
}

void ChassisDomainServer::stop()
{
    running_ = false;
    if (listen_sock_ != SOCK_INVALID) {
        SOCK_CLOSE(listen_sock_);
        listen_sock_ = SOCK_INVALID;
    }
}

void ChassisDomainServer::handle_connection(sock_t conn)
{
    std::printf("[chassis] 新连接\n");
    try {
        MsgType     msg_type;
        std::string payload;
        recv_frame(conn, msg_type, payload);
        std::printf("[chassis] 收到帧 type=%u\n",
                    static_cast<unsigned>(msg_type));

        nlohmann::json j_payload;
        if (!payload.empty())
            j_payload = nlohmann::json::parse(payload);

        switch (msg_type) {

            case MsgType::CHASSIS_GET_READINESS_REQ: {
                auto rsp = handler_->get_readiness(j_payload);
                nlohmann::json j_rsp = rsp;
                send_frame(conn, MsgType::CHASSIS_GET_READINESS_RSP, j_rsp.dump());
                break;
            }

            case MsgType::CHASSIS_MOTION_PRIMITIVE_REQ: {
                MotionPrimitiveRequest req;
                from_json(j_payload, req);
                auto rsp = handler_->execute_motion_primitive(req);
                nlohmann::json j_rsp = rsp;
                send_frame(conn, MsgType::CHASSIS_MOTION_PRIMITIVE_RSP, j_rsp.dump());
                break;
            }

            case MsgType::CHASSIS_EMERGENCY_BRAKE_REQ: {
                auto j_rsp = handler_->emergency_brake(j_payload);
                send_frame(conn, MsgType::CHASSIS_EMERGENCY_BRAKE_RSP, j_rsp.dump());
                break;
            }

            default:
                std::printf("[chassis] 未知帧类型 %u，忽略\n",
                            static_cast<unsigned>(msg_type));
                break;
        }
    } catch (const std::exception& e) {
        std::printf("[chassis] 处理帧异常: %s\n", e.what());
    }
    SOCK_CLOSE(conn);
}

}  // namespace vaw
