#!/usr/bin/env python3
"""P19 XDM-60T4-E Y1..Y4 -> X0..X3 physical loopback tests."""

from __future__ import annotations

import argparse
import struct
import time

import serial

from plsr_modbus_control_test import (
    CALL_COMMIT,
    CALL_START,
    CMD_STOP_DECEL,
    CONTROL_BASE,
    CONTROL_WINDOW_WORDS,
    RESULT_OK,
    RESULT_QUEUED,
    S0_BASE,
    S1_BASE,
    check_result,
    read_axis_status,
    send_call,
    send_command,
)
from plsr_modbus_frequency_test import RtuClient, choose_port, signed_dword_words


LOOPBACK_BASE = 1540
LOOPBACK_SIGNATURE = 0x5019
STATE_IDLE = 1
STATE_ACCEL = 2
STATE_RUN = 3
STATE_WAIT = 5
STATE_COMPLETED = 7
STATE_STOPPED = 8
CMD_RESET_ERROR = 10
ERROR_LIMIT_POSITIVE = 6
ERROR_LIMIT_NEGATIVE = 7
STOP_LIMIT_POSITIVE = 5
STOP_LIMIT_NEGATIVE = 6


def read_bits(client: RtuClient, address: int, count: int) -> list[int]:
    byte_count = (count + 7) // 8
    pdu = bytes((0x02,)) + struct.pack(">HH", address, count)
    response = client.exchange(pdu, 5 + byte_count)
    if response[1] != 0x02 or response[2] != byte_count:
        raise RuntimeError(f"FC02 响应格式错误：{response.hex(' ')}")
    return [
        (response[3 + index // 8] >> (index % 8)) & 1
        for index in range(count)
    ]


class LoopbackLease:
    def __init__(self, client: RtuClient) -> None:
        self.client = client
        self.sequence = int(time.time() * 1000) & 0xFFFF
        self.mask = 0
        self.last_write = 0.0

    def set(self, mask: int) -> None:
        self.mask = mask & 0x0F
        self.sequence = (self.sequence + 1) & 0xFFFF
        self.client.write_multiple(LOOPBACK_BASE, [self.mask, self.sequence])
        self.last_write = time.monotonic()

    def heartbeat(self) -> None:
        if time.monotonic() - self.last_write >= 0.20:
            self.set(self.mask)

    def diagnostics(self) -> dict[str, int]:
        words = self.client.read_holding(LOOPBACK_BASE + 2, 5)
        return {
            "applied": words[0],
            "raw": words[1],
            "logical": words[2],
            "signature": words[3],
            "flags": words[4],
        }


def wait_until(
    lease: LoopbackLease,
    predicate,
    timeout: float,
    label: str,
) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        lease.heartbeat()
        latest = read_axis_status(lease.client)
        if predicate(latest):
            return latest
    raise RuntimeError(f"{label}超时，最后状态={latest}")


def prepare_job(
    client: RtuClient,
    pulses: int,
    wait_condition: int = 0,
    wait_source: int = 0,
    wait_point: int = 0,
) -> None:
    words = [0] * 20
    words[0:2] = signed_dword_words(1)
    words[10:12] = signed_dword_words(2000)
    words[12:14] = signed_dword_words(pulses)
    words[14] = ((wait_condition & 0xFF) << 8) | (wait_source & 0xFF)
    words[15:17] = signed_dword_words(wait_point)
    words[17] = 0
    words[18:20] = signed_dword_words(0)
    client.write_multiple(S0_BASE, words)
    client.write_multiple(S1_BASE, [0, 0, 0, 0])


def start_job(client: RtuClient, sequence: int) -> int:
    response = send_call(client, sequence, CALL_COMMIT)
    check_result(response, RESULT_OK, "COMMIT")
    response = send_call(client, sequence + 1, CALL_START)
    check_result(response, RESULT_QUEUED, "START")
    return sequence + 1


def reset_limit_error(lease: LoopbackLease, sequence: int) -> None:
    lease.set(0)
    time.sleep(0.050)
    response = send_command(lease.client, sequence, CMD_RESET_ERROR)
    check_result(response, RESULT_QUEUED, "RESET_ERROR")
    status = wait_until(
        lease,
        lambda value: value["state"] == STATE_IDLE
        and value["last_sequence"] == sequence,
        2.0,
        "RESET_ERROR",
    )
    if status["error"] != 0:
        raise RuntimeError(f"RESET_ERROR 后错误未清除：{status}")


def test_electrical(lease: LoopbackLease) -> None:
    patterns = (0x0, 0x1, 0x2, 0x4, 0x8, 0x3, 0xC, 0xF, 0x0)
    for pattern in patterns:
        lease.set(pattern)
        time.sleep(0.050)
        diagnostics = lease.diagnostics()
        x_bits = read_bits(lease.client, 0, 4)
        observed = sum(bit << index for index, bit in enumerate(x_bits))
        if diagnostics["signature"] != LOOPBACK_SIGNATURE:
            raise RuntimeError(f"不是 P19 固件：{diagnostics}")
        if (diagnostics["flags"] & 0x0013) != 0x0013:
            raise RuntimeError(f"P19 输入/租约未就绪：{diagnostics}")
        if diagnostics["applied"] != pattern or observed != pattern:
            raise RuntimeError(
                f"回环不一致：写Y掩码=0x{pattern:X}, FC02 X=0x{observed:X}, "
                f"诊断={diagnostics}。请检查对应 Y→X 和 COM 接线。"
            )
        print(
            f"Y1~Y4=0x{pattern:X} -> X0~X3=0x{observed:X} PASS "
            f"(MCU raw=0x{diagnostics['raw'] & 0xF:X})"
        )
    print("电气回环 PASS：Y1→X0、Y2→X1、Y3→X2、Y4→X3")


def test_wait(lease: LoopbackLease, sequence: int) -> None:
    lease.set(0)
    prepare_job(lease.client, 500, wait_condition=2, wait_source=4, wait_point=3)
    start_sequence = start_job(lease.client, sequence)
    waiting = wait_until(
        lease,
        lambda value: value["state"] == STATE_WAIT,
        3.0,
        "进入 X3 WAIT",
    )
    if waiting["task_pulses"] != 500:
        raise RuntimeError(f"WAIT 前脉冲数不是500：{waiting}")
    lease.set(0x8)
    completed = wait_until(
        lease,
        lambda value: value["state"] == STATE_COMPLETED,
        2.0,
        "X3 释放 WAIT",
    )
    if completed["task_pulses"] != 500 or completed["error"] != 0:
        raise RuntimeError(f"WAIT 完成状态异常：{completed}")
    lease.set(0)
    print(f"X3/WAIT PASS：500脉冲后等待，Y4→X3释放，序号={start_sequence}")


def test_ext(lease: LoopbackLease, sequence: int) -> None:
    lease.set(0)
    prepare_job(lease.client, 50000, wait_condition=4, wait_source=4, wait_point=2)
    start_job(lease.client, sequence)
    running = wait_until(
        lease,
        lambda value: value["state"] in {STATE_ACCEL, STATE_RUN}
        and abs(value["task_pulses"]) >= 200,
        3.0,
        "EXT 触发前运行",
    )
    lease.set(0x4)
    completed = wait_until(
        lease,
        lambda value: value["state"] == STATE_COMPLETED,
        2.0,
        "X2 EXT 上升沿切段",
    )
    lease.set(0)
    if not (running["task_pulses"] <= completed["task_pulses"] < 50000):
        raise RuntimeError(f"EXT 未提前结束任务：{completed}")
    if completed["error"] != 0:
        raise RuntimeError(f"EXT 完成存在错误：{completed}")
    print(f"X2/EXT PASS：Y3→X2 上升沿在 {completed['task_pulses']} 脉冲提前结束")


def test_limit(
    lease: LoopbackLease,
    sequence: int,
    positive: bool,
) -> None:
    lease.set(0)
    pulses = 50000 if positive else -50000
    bit_mask = 0x1 if positive else 0x2
    expected_error = ERROR_LIMIT_POSITIVE if positive else ERROR_LIMIT_NEGATIVE
    expected_reason = STOP_LIMIT_POSITIVE if positive else STOP_LIMIT_NEGATIVE
    flag_mask = 1 << (9 if positive else 10)
    label = "正硬限位 X0" if positive else "负硬限位 X1"
    prepare_job(lease.client, pulses)
    start_job(lease.client, sequence)
    wait_until(
        lease,
        lambda value: value["state"] in {STATE_ACCEL, STATE_RUN}
        and abs(value["task_pulses"]) >= 200,
        3.0,
        f"{label}触发前运行",
    )
    lease.set(bit_mask)
    stopped = wait_until(
        lease,
        lambda value: value["state"] == STATE_STOPPED,
        3.0,
        f"{label}停止",
    )
    if (
        stopped["error"] != expected_error
        or stopped["stop_reason"] != expected_reason
        or (stopped["flags"] & flag_mask) == 0
        or abs(stopped["task_pulses"]) >= 50000
    ):
        raise RuntimeError(f"{label}状态不符合预期：{stopped}")
    reset_limit_error(lease, sequence + 100)
    print(
        f"{label} PASS：提前停止于 {stopped['task_pulses']} 脉冲，"
        f"error={expected_error}, stop_reason={expected_reason}"
    )


def main() -> int:
    parser = argparse.ArgumentParser(
        description="P19 XDM-60T4-E Y1~Y4→X0~X3真实输入、WAIT/EXT/硬限位测试"
    )
    parser.add_argument("--port", default="COM5")
    parser.add_argument("--baud", type=int, default=9600)
    parser.add_argument("--slave", type=int, default=1)
    parser.add_argument(
        "--case",
        choices=("electrical", "wait", "ext", "positive-limit", "negative-limit", "all"),
        default="electrical",
    )
    args = parser.parse_args()

    with serial.Serial(
        port=choose_port(args.port),
        baudrate=args.baud,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_EVEN,
        stopbits=serial.STOPBITS_ONE,
        timeout=1.0,
        write_timeout=1.0,
    ) as uart:
        client = RtuClient(uart, args.slave)
        header = client.read_holding(CONTROL_BASE, 8)
        if header[3] != CONTROL_WINDOW_WORDS:
            raise RuntimeError(f"控制窗口不匹配：{header}")
        lease = LoopbackLease(client)
        sequence = int(time.time()) & 0x7FFFFFFF
        try:
            cases = (
                ("electrical", lambda: test_electrical(lease)),
                ("wait", lambda: test_wait(lease, sequence + 10)),
                ("ext", lambda: test_ext(lease, sequence + 20)),
                ("positive-limit", lambda: test_limit(lease, sequence + 30, True)),
                ("negative-limit", lambda: test_limit(lease, sequence + 40, False)),
            )
            for case_name, operation in cases:
                if args.case in {case_name, "all"}:
                    print(f"\n开始 P19 用例：{case_name}")
                    operation()
            print("\n全部所选 P19 自动检查 PASS")
        finally:
            try:
                lease.set(0)
            except (RuntimeError, serial.SerialException):
                pass
    return 0


if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except (RuntimeError, serial.SerialException) as error:
        print(f"测试失败：{error}")
        raise SystemExit(1)