"""frame_codec.py

TCP 帧编解码层。

帧格式（与 Ubuntu 侧 proto_frame.cpp 完全一致）：
  [4字节 LE uint32: msg_type][4字节 LE uint32: payload_len][payload_len字节: UTF-8 JSON]

所有公共 JSON 字段的键名与 proto 字段名对齐，Ubuntu 侧 gateway_codec.hpp 是权威参考。
"""

import json
import socket
import struct
from enum import IntEnum
from typing import Any, Dict, Tuple


# ---------------------------------------------------------------------------
# 消息类型枚举（与 proto_frame.hpp MsgType 完全一致）
# ---------------------------------------------------------------------------
class MsgType(IntEnum):
    # 底盘域
    CHASSIS_GET_READINESS_REQ   = 1
    CHASSIS_GET_READINESS_RSP   = 2
    CHASSIS_MOTION_PRIMITIVE_REQ = 3
    CHASSIS_MOTION_PRIMITIVE_RSP = 4
    CHASSIS_EMERGENCY_BRAKE_REQ  = 5
    CHASSIS_EMERGENCY_BRAKE_RSP  = 6
    # 运控域
    CONTROL_GET_READINESS_REQ   = 11
    CONTROL_GET_READINESS_RSP   = 12
    CONTROL_EVALUATION_REQ      = 13
    CONTROL_EVALUATION_RSP      = 14


# ---------------------------------------------------------------------------
# 底盘动作原语类型（与 ChassisMotionPrimitiveType.msg 一致）
# ---------------------------------------------------------------------------
class ChassisMotionPrimitiveType(IntEnum):
    UNSPECIFIED          = 0
    STRAIGHT_LINE        = 1
    ARC                  = 2
    IN_PLACE_ROTATION    = 3
    STEERING_SWEEP       = 4
    LATERAL_TRANSLATION  = 5
    DIAGONAL_MOTION      = 6
    MODULE_ALIGNMENT     = 7
    COORDINATED_STEERING = 8


# ---------------------------------------------------------------------------
# 运控评估任务类型（与 ControllerEvaluationTaskType.msg 一致）
# ---------------------------------------------------------------------------
class ControlEvaluationTaskType(IntEnum):
    UNSPECIFIED              = 0
    TRAJECTORY_TRACKING      = 1
    VELOCITY_STEP            = 2
    ACCELERATION_DECELERATION = 3
    STOP_ACCURACY            = 4


# ---------------------------------------------------------------------------
# ErrorCode（与 ErrorCode.msg 一致）
# ---------------------------------------------------------------------------
class ErrorCode(IntEnum):
    UNSPECIFIED               = 0
    OK                        = 1
    INVALID_ARGUMENT          = 2
    INVALID_STATE             = 3
    VEHICLE_BUSY              = 4
    NOT_READY                 = 5
    TIMEOUT                   = 6
    NETWORK_LOSS              = 7
    SAFETY_TRIGGERED          = 8
    HARDWARE_FAULT            = 9
    FILE_NOT_FOUND            = 10
    CHECKSUM_MISMATCH         = 11
    INTERNAL_ERROR            = 12
    UNSUPPORTED_CAPABILITY    = 13
    RESOURCE_LOCKED           = 14
    MANUAL_CONFIRM_REQUIRED   = 15
    APPROVAL_REQUIRED         = 16
    VALIDATION_FAILED         = 17
    ROLLBACK_REQUIRED         = 18
    DATA_QUALITY_INSUFFICIENT = 19


# ---------------------------------------------------------------------------
# 底层 I/O 工具
# ---------------------------------------------------------------------------

def _recv_exactly(conn: socket.socket, n: int) -> bytes:
    """从 conn 读取恰好 n 字节，连接关闭时抛出 ConnectionError。"""
    buf = bytearray()
    while len(buf) < n:
        chunk = conn.recv(n - len(buf))
        if not chunk:
            raise ConnectionError("连接已关闭")
        buf.extend(chunk)
    return bytes(buf)


def recv_frame(conn: socket.socket) -> Tuple[MsgType, Dict[str, Any]]:
    """从 conn 读一帧，返回 (msg_type, payload_dict)。

    payload_dict 为空 dict 表示 payload_len=0 的帧。
    """
    header = _recv_exactly(conn, 8)
    msg_type_val, payload_len = struct.unpack_from("<II", header)
    msg_type = MsgType(msg_type_val)
    if payload_len == 0:
        return msg_type, {}
    raw = _recv_exactly(conn, payload_len)
    payload = json.loads(raw.decode("utf-8"))
    return msg_type, payload


def send_frame(conn: socket.socket, msg_type: MsgType, payload: Dict[str, Any]) -> None:
    """向 conn 发一帧。payload 为空 dict 时 payload_len=0。"""
    if payload:
        data = json.dumps(payload, ensure_ascii=False).encode("utf-8")
    else:
        data = b""
    header = struct.pack("<II", int(msg_type), len(data))
    conn.sendall(header + data)


# ---------------------------------------------------------------------------
# 公共响应构造辅助
# ---------------------------------------------------------------------------

def make_error_code(code: ErrorCode) -> Dict[str, int]:
    return {"code": int(code)}


def make_success_response(message: str = "") -> Dict[str, Any]:
    return {
        "success": True,
        "error_code": make_error_code(ErrorCode.OK),
        "message": message,
    }


def make_failure_response(message: str, code: ErrorCode = ErrorCode.INTERNAL_ERROR) -> Dict[str, Any]:
    return {
        "success": False,
        "error_code": make_error_code(code),
        "message": message,
    }


# ---------------------------------------------------------------------------
# Readiness 响应构造辅助
# ---------------------------------------------------------------------------

def make_chassis_readiness_response(
    *,
    success: bool,
    message: str,
    agent_ready: bool,
    chassis_driver_online: bool,
    motion_control_ready: bool,
    estop_released: bool,
    vehicle_safe_to_move: bool,
    telemetry_available: bool,
    checked_timestamp_us: int,
    error_code: ErrorCode = ErrorCode.OK,
) -> Dict[str, Any]:
    """构造底盘就绪响应 payload（对应 ChassisReadinessResponse.msg）。"""
    return {
        "success": success,
        "error_code": make_error_code(error_code),
        "message": message,
        "agent_ready": agent_ready,
        "chassis_driver_online": chassis_driver_online,
        "motion_control_ready": motion_control_ready,
        "estop_released": estop_released,
        "vehicle_safe_to_move": vehicle_safe_to_move,
        "telemetry_available": telemetry_available,
        "checked_timestamp_us": checked_timestamp_us,
    }


def make_control_readiness_response(
    *,
    success: bool,
    message: str,
    agent_ready: bool,
    trajectory_executor_ready: bool,
    vehicle_feedback_ready: bool,
    control_output_ready: bool,
    estop_released: bool,
    vehicle_safe_to_move: bool,
    checked_timestamp_us: int,
    external_pose_feedback_ready: bool = False,
    external_pose_source_name: str = "",
    external_pose_age_ms: float = 0.0,
    external_pose_quality_score: float = 0.0,
    external_pose_transport: str = "",
    error_code: ErrorCode = ErrorCode.OK,
) -> Dict[str, Any]:
    """构造运控就绪响应 payload（对应 ControlReadinessResponse.msg）。"""
    return {
        "success": success,
        "error_code": make_error_code(error_code),
        "message": message,
        "agent_ready": agent_ready,
        "trajectory_executor_ready": trajectory_executor_ready,
        "vehicle_feedback_ready": vehicle_feedback_ready,
        "control_output_ready": control_output_ready,
        "estop_released": estop_released,
        "vehicle_safe_to_move": vehicle_safe_to_move,
        "checked_timestamp_us": checked_timestamp_us,
        "external_pose_feedback_ready": external_pose_feedback_ready,
        "external_pose_source_name": external_pose_source_name,
        "external_pose_age_ms": external_pose_age_ms,
        "external_pose_quality_score": external_pose_quality_score,
        "external_pose_transport": external_pose_transport,
    }


# ---------------------------------------------------------------------------
# 任务结果响应构造辅助
# ---------------------------------------------------------------------------

def make_chassis_job_result(
    *,
    success: bool,
    message: str,
    job_id: str,
    data_quality_passed: bool,
    suitable_for_commit: bool,
    recommended_parameter_version: str = "",
    estimated_straight_line_bias: float = 0.0,
    max_lateral_error_m: float = 0.0,
    max_yaw_error_rad: float = 0.0,
    rms_lateral_error_m: float = 0.0,
    rms_yaw_error_rad: float = 0.0,
    repeatability_error_m: float = 0.0,
    curvature_error: float = 0.0,
    module_consistency_error: float = 0.0,
    auto_acceptance_passed: bool = False,
    artifacts: list = None,
    error_code: ErrorCode = ErrorCode.OK,
) -> Dict[str, Any]:
    """构造底盘任务结果 payload（对应 ChassisJobResult.msg）。"""
    return {
        "success": success,
        "error_code": make_error_code(error_code if success else ErrorCode.INTERNAL_ERROR),
        "message": message,
        "job_id": job_id,
        "data_quality_passed": data_quality_passed,
        "suitable_for_commit": suitable_for_commit,
        "recommended_parameter_version": recommended_parameter_version,
        "estimated_straight_line_bias": estimated_straight_line_bias,
        "validation_summary": {
            "max_lateral_error_m": max_lateral_error_m,
            "max_yaw_error_rad": max_yaw_error_rad,
            "rms_lateral_error_m": rms_lateral_error_m,
            "rms_yaw_error_rad": rms_yaw_error_rad,
            "repeatability_error_m": repeatability_error_m,
            "curvature_error": curvature_error,
            "module_consistency_error": module_consistency_error,
            "auto_acceptance_passed": auto_acceptance_passed,
        },
        "artifacts": artifacts or [],
    }


def make_control_job_result(
    *,
    success: bool,
    message: str,
    job_id: str,
    data_quality_passed: bool,
    suitable_for_commit: bool,
    recommended_parameter_version: str = "",
    rms_lateral_error_m: float = 0.0,
    rms_heading_error_rad: float = 0.0,
    rms_speed_error_ms: float = 0.0,
    overshoot_ratio: float = 0.0,
    settle_time_sec: float = 0.0,
    stop_position_error_m: float = 0.0,
    max_jerk: float = 0.0,
    saturation_ratio: float = 0.0,
    auto_acceptance_passed: bool = False,
    artifacts: list = None,
    error_code: ErrorCode = ErrorCode.OK,
) -> Dict[str, Any]:
    """构造运控任务结果 payload（对应 ControlJobResult.msg）。"""
    return {
        "success": success,
        "error_code": make_error_code(error_code if success else ErrorCode.INTERNAL_ERROR),
        "message": message,
        "job_id": job_id,
        "data_quality_passed": data_quality_passed,
        "suitable_for_commit": suitable_for_commit,
        "recommended_parameter_version": recommended_parameter_version,
        "validation_summary": {
            "rms_lateral_error_m": rms_lateral_error_m,
            "rms_heading_error_rad": rms_heading_error_rad,
            "rms_speed_error_ms": rms_speed_error_ms,
            "overshoot_ratio": overshoot_ratio,
            "settle_time_sec": settle_time_sec,
            "stop_position_error_m": stop_position_error_m,
            "max_jerk": max_jerk,
            "saturation_ratio": saturation_ratio,
            "auto_acceptance_passed": auto_acceptance_passed,
        },
        "artifacts": artifacts or [],
    }


def make_artifact(file_name: str, file_uri: str, size_bytes: int = 0, description: str = "") -> Dict[str, Any]:
    """构造文件引用条目（对应 FileReference.msg）。"""
    return {
        "file_name": file_name,
        "file_uri": file_uri,
        "size_bytes": size_bytes,
        "description": description,
    }
