#!/usr/bin/env python3
"""提取 2026042802-1 抓包中的真实 J519 发包，并对比 UTTC_MS11 示教点。"""

from __future__ import annotations

import csv
import json
import math
import struct
import subprocess
from collections import Counter
from pathlib import Path


REPO_ROOT = Path(__file__).resolve().parents[1]
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
RUNTIME_DATA_DIR = REPO_ROOT / "Config" / "Data" / "UTTC_MS11"


def be_u32(data: bytes, offset: int) -> int:
    """按大端读取 4 字节无符号整数。"""
    return struct.unpack(">I", data[offset : offset + 4])[0]


def be_u16(data: bytes, offset: int) -> int:
    """按大端读取 2 字节无符号整数。"""
    return struct.unpack(">H", data[offset : offset + 2])[0]


def be_f32(data: bytes, offset: int) -> float:
    """按大端读取 4 字节浮点数。"""
    return struct.unpack(">f", data[offset : offset + 4])[0]


def load_j519_command_rows(pcap: Path, tshark: Path) -> list[list[str]]:
    """只提取 UDP 60015 的原始字段，后续再按 IP 方向筛选真实下发命令。"""
    command = [
        str(tshark),
        "-r",
        str(pcap),
        "-Y",
        "udp.port==60015",
        "-T",
        "fields",
        "-e",
        "frame.number",
        "-e",
        "frame.time_relative",
        "-e",
        "ip.src",
        "-e",
        "udp.srcport",
        "-e",
        "ip.dst",
        "-e",
        "udp.dstport",
        "-e",
        "udp.payload",
    ]
    output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
    rows: list[list[str]] = []
    for line in output.splitlines():
        if not line.strip():
            continue
        parts = line.split("\t")
        if len(parts) >= 7:
            rows.append(parts[:7])
    return rows


def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
    """把抓包中的 64B J519 命令帧解码成带 IO 信息的结构化记录。"""
    records: list[dict] = []
    for frame_no, time_rel, ip_src, _udp_src, ip_dst, _udp_dst, payload_hex in rows:
        if ip_src != client_ip or ip_dst != robot_ip:
            continue

        payload = bytes.fromhex(payload_hex)
        if len(payload) != 64:
            continue

        io_value = be_u16(payload, 0x18)
        io_addrs = [bit + 1 for bit in range(16) if io_value & (1 << bit)]
        targets = [be_f32(payload, 0x1C + index * 4) for index in range(9)]
        records.append(
            {
                "frame_number": int(frame_no),
                "time_relative_s": float(time_rel),
                "sequence": be_u32(payload, 0x08),
                "last_data": payload[0x0C],
                "write_io_type": payload[0x13],
                "write_io_index": be_u16(payload, 0x14),
                "write_io_mask": be_u16(payload, 0x16),
                "write_io_value": io_value,
                "io_addrs": io_addrs,
                "j1_deg": targets[0],
                "j2_deg": targets[1],
                "j3_deg": targets[2],
                "j4_deg": targets[3],
                "j5_deg": targets[4],
                "j6_deg": targets[5],
                "ext1_deg": targets[6],
                "ext2_deg": targets[7],
                "ext3_deg": targets[8],
            }
        )
    return records


def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
    """由于 io_keep_cycles=2，只记录每组高电平脉冲的第一帧。"""
    trigger_frames: list[dict] = []
    previous_high = False
    for record in records:
        current_high = record["write_io_value"] > 0
        if current_high and not previous_high:
            trigger_frames.append(record)
        previous_high = current_high
    return trigger_frames


def load_uttc_ms11_config() -> dict:
    """读取 UTTC_MS11 的示教点和触发配置。"""
    config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
    return config["flying_shots"]["UTTC_MS11"]


def build_trigger_vs_teach_rows(trigger_frames: list[dict], shot_config: dict) -> list[dict]:
    """按 shot_flags 为 true 的 waypoint 顺序，对齐抓包触发帧和示教点。"""
    rows: list[dict] = []
    trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]

    for trigger_no, (frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
        teach_rad = shot_config["traj_waypoints"][waypoint_index]
        teach_deg = [math.degrees(value) for value in teach_rad]
        actual_deg = [frame[f"j{joint_index}_deg"] for joint_index in range(1, 7)]
        diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
        abs_diffs = [abs(value) for value in diffs]
        max_error = max(abs_diffs)
        max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
        rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)

        row = {
            "trigger_no": trigger_no,
            "waypoint_index": waypoint_index,
            "frame_number": frame["frame_number"],
            "sequence": frame["sequence"],
            "time_relative_s": frame["time_relative_s"],
            "write_io_value": frame["write_io_value"],
            "io_addrs": frame["io_addrs"],
            "config_addr": shot_config["addr"][waypoint_index],
            "max_error_axis": max_error_axis,
            "max_error_deg": max_error,
            "rms_error_deg": rms_error,
        }

        for joint_index in range(6):
            joint_no = joint_index + 1
            row[f"j{joint_no}_actual_deg"] = actual_deg[joint_index]
            row[f"j{joint_no}_teach_deg"] = teach_deg[joint_index]
            row[f"diff_j{joint_no}_deg"] = diffs[joint_index]

        rows.append(row)

    return rows


def write_csv(path: Path, rows: list[dict]) -> None:
    """把分析结果落成 UTF-8 CSV，便于后续继续筛选和画图。"""
    if not rows:
        raise ValueError(f"No rows to write: {path}")

    serializable_rows: list[dict] = []
    for row in rows:
        serializable_row: dict = {}
        for key, value in row.items():
            if isinstance(value, list):
                serializable_row[key] = json.dumps(value, ensure_ascii=False)
            else:
                serializable_row[key] = value
        serializable_rows.append(serializable_row)

    path.parent.mkdir(parents=True, exist_ok=True)
    with path.open("w", newline="", encoding="utf-8") as handle:
        writer = csv.DictWriter(handle, fieldnames=list(serializable_rows[0].keys()))
        writer.writeheader()
        writer.writerows(serializable_rows)


def build_summary(records: list[dict], trigger_rows: list[dict]) -> dict:
    """汇总本次分析关心的导出状态和误差统计。"""
    max_errors = [float(row["max_error_deg"]) for row in trigger_rows]
    rms_errors = [float(row["rms_error_deg"]) for row in trigger_rows]
    axis_counter = Counter(str(row["max_error_axis"]) for row in trigger_rows)

    order_only_addr_mismatch = 0
    real_addr_mismatch = 0
    for row in trigger_rows:
        io_addrs = list(row["io_addrs"])
        config_addr = list(row["config_addr"])
        if io_addrs != config_addr:
            if sorted(io_addrs) == sorted(config_addr):
                order_only_addr_mismatch += 1
            else:
                real_addr_mismatch += 1

    return {
        "pcap_path": str(DEFAULT_PCAP),
        "all_command_count": len(records),
        "trigger_count": len(trigger_rows),
        "existing_runtime_actual_send_exists": (RUNTIME_DATA_DIR / "ActualSendJointTraj.txt").exists(),
        "existing_runtime_actual_send_has_io_columns": False,
        "existing_shot_events_exists": (RUNTIME_DATA_DIR / "ShotEvents.json").exists(),
        "pcap_specific_combined_export_preexisting": False,
        "average_max_error_deg": sum(max_errors) / len(max_errors),
        "max_error_deg": max(max_errors),
        "average_rms_error_deg": sum(rms_errors) / len(rms_errors),
        "max_error_axis_counter": dict(axis_counter),
        "order_only_addr_mismatch_count": order_only_addr_mismatch,
        "real_addr_mismatch_count": real_addr_mismatch,
    }


def main() -> None:
    """执行抓包提取、示教点对齐、CSV 导出和摘要落盘。"""
    OUTPUT_DIR.mkdir(parents=True, exist_ok=True)

    rows = load_j519_command_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
    records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
    trigger_frames = pick_trigger_first_high_frames(records)
    shot_config = load_uttc_ms11_config()
    trigger_rows = build_trigger_vs_teach_rows(trigger_frames, shot_config)
    summary = build_summary(records, trigger_rows)

    write_csv(OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv", records)
    write_csv(OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv", trigger_frames)
    write_csv(OUTPUT_DIR / "2026042802-1_trigger_vs_teach_points.csv", trigger_rows)
    (OUTPUT_DIR / "2026042802-1_analysis_summary.json").write_text(
        json.dumps(summary, ensure_ascii=False, indent=2),
        encoding="utf-8",
    )

    print(json.dumps(summary, ensure_ascii=False, indent=2))


if __name__ == "__main__":
    main()
