// vehicle_gateway_server.cpp
// 车端统一 WiFi6/TCP 入口，按消息类型分发到底盘、运控和外部定位位姿缓存。

#include "vehicle_gateway_server.hpp"

#include <cstdio>
#include <stdexcept>
#include <string>
#include <thread>

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

namespace vaw
{

VehicleGatewayServer::VehicleGatewayServer(
    uint16_t port,
    ChassisHandler* chassis_handler,
    ControlHandler* control_handler,
    ExternalPoseStore* external_pose_store)
    : port_(port),
      chassis_handler_(chassis_handler),
      control_handler_(control_handler),
      external_pose_store_(external_pose_store),
      running_(false),
      listen_sock_(SOCK_INVALID)
{}

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

void VehicleGatewayServer::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_, 32);
    running_ = true;
    std::printf("[gateway] 监听端口 %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;

        std::thread([this, conn]() { handle_connection(conn); }).detach();
    }
}

void VehicleGatewayServer::stop()
{
    running_ = false;
    if (listen_sock_ != SOCK_INVALID) {
        ::shutdown(listen_sock_, SOCK_SHUTDOWN_BOTH);
        SOCK_CLOSE(listen_sock_);
        listen_sock_ = SOCK_INVALID;
    }
}

MsgType VehicleGatewayServer::response_type_for(MsgType request_type) const
{
    switch (request_type) {
        case MsgType::CHASSIS_GET_READINESS_REQ:
            return MsgType::CHASSIS_GET_READINESS_RSP;
        case MsgType::CHASSIS_MOTION_PRIMITIVE_REQ:
            return MsgType::CHASSIS_MOTION_PRIMITIVE_RSP;
        case MsgType::CHASSIS_EMERGENCY_BRAKE_REQ:
            return MsgType::CHASSIS_EMERGENCY_BRAKE_RSP;
        case MsgType::CONTROL_GET_READINESS_REQ:
            return MsgType::CONTROL_GET_READINESS_RSP;
        case MsgType::CONTROL_EVALUATION_REQ:
            return MsgType::CONTROL_EVALUATION_RSP;
        case MsgType::EXTERNAL_POSE_PUSH_REQ:
            return MsgType::EXTERNAL_POSE_PUSH_RSP;
        default:
            return MsgType::UNSPECIFIED;
    }
}

void VehicleGatewayServer::send_error_response(sock_t conn, MsgType response_type, const std::string& message) const
{
    StandardResponse rsp;
    rsp.success = false;
    rsp.error_code = ErrorCode::INVALID_ARGUMENT;
    rsp.message = message;
    nlohmann::json j_rsp = rsp;
    send_frame(conn, response_type, j_rsp.dump());
}

void VehicleGatewayServer::handle_connection(sock_t conn)
{
    std::printf("[gateway] 新连接\n");
    MsgType msg_type = MsgType::UNSPECIFIED;
    try {
        std::string payload;
        recv_frame(conn, msg_type, payload);
        std::printf("[gateway] 收到帧 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 = chassis_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 = chassis_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 = chassis_handler_->emergency_brake(j_payload);
                send_frame(conn, MsgType::CHASSIS_EMERGENCY_BRAKE_RSP, j_rsp.dump());
                break;
            }

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

            case MsgType::CONTROL_EVALUATION_REQ: {
                ControllerEvaluationRequest req;
                from_json(j_payload, req);
                auto rsp = control_handler_->execute_evaluation(req);
                nlohmann::json j_rsp = rsp;
                send_frame(conn, MsgType::CONTROL_EVALUATION_RSP, j_rsp.dump());
                break;
            }

            case MsgType::EXTERNAL_POSE_PUSH_REQ: {
                ExternalLocalizationTelemetry req;
                from_json(j_payload, req);
                auto rsp = external_pose_store_->update(req);
                nlohmann::json j_rsp = rsp;
                send_frame(conn, MsgType::EXTERNAL_POSE_PUSH_RSP, j_rsp.dump());
                break;
            }

            default:
                std::printf("[gateway] 未知帧类型 %u，忽略\n", static_cast<unsigned>(msg_type));
                send_error_response(
                    conn,
                    MsgType::UNSPECIFIED,
                    "不支持的统一入口帧类型: " + std::to_string(static_cast<unsigned>(msg_type)));
                break;
        }
    } catch (const std::exception& e) {
        std::printf("[gateway] 处理帧异常: %s\n", e.what());
        send_error_response(conn, response_type_for(msg_type), e.what());
    }
    SOCK_CLOSE(conn);
}

}  // namespace vaw
