#!/usr/bin/env python3
"""PLSR P15 four-axis positive/negative soft-limit precision test."""

from __future__ import annotations

import argparse
import time

import serial

from plsr_modbus_counter_stress_test import (
    CALL_COMMIT,
    CALL_START,
    CMD_SET_POSITION,
    CONTROL_BASE,
    CONTROL_WINDOW_WORDS,
    RESULT_OK,
    RESULT_QUEUED,
    RtuClient,
    S0_BASES,
    S1_BASES,
    check_result,
    put_u32,
    read_axis_status,
    send_call,
    send_command,
)
from plsr_modbus_frequency_test import choose_port


STATE_ACCEL = 2
STATE_RUN = 3
STATE_STOPPED = 8
STATE_IDLE = 1
CMD_RESET_ERROR = 10
ERROR_LIMIT_POSITIVE = 6
ERROR_LIMIT_NEGATIVE = 7
STOP_LIMIT_POSITIVE = 5
STOP_LIMIT_NEGATIVE = 6
SOFT_LIMIT = 1_000_000


CASES = (
    # axis, start position, frequency, requested pulses, expected output pulses
    (0, 999_800, 500, 10_000, 200),
    (1, 999_000, 2_000, 10_000, 1_000),
    (2, -999_800, 500, -10_000, 200),
    (3, -999_000, 2_000, -10_000, 1_000),
)


def wait_command_applied(
    client: RtuClient,
    axis: int,
    sequence: int,
    expected_result: int = RESULT_OK,
    timeout: float = 3.0,
) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_axis_status(client, axis)
        if latest["last_sequence"] == sequence:
            if latest["last_result"] != expected_result:
                raise RuntimeError(
                    f"轴{axis}命令#{sequence}执行结果={latest['last_result']}，"
                    f"期望={expected_result}；状态={latest}"
                )
            return latest
    raise RuntimeError(f"等待轴{axis}命令#{sequence}执行超时，最后状态={latest}")


def wait_state(
    client: RtuClient, axis: int, expected: set[int], timeout: float = 6.0
) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_axis_status(client, axis)
        if latest["state"] in expected:
            return latest
    raise RuntimeError(
        f"等待轴{axis}状态{sorted(expected)}超时，最后状态={latest}"
    )


def write_job(
    client: RtuClient, axis: int, frequency_hz: int, signed_pulses: int
) -> None:
    s0 = [0] * 20
    put_u32(s0, 0, 1)
    put_u32(s0, 10, frequency_hz)
    put_u32(s0, 12, signed_pulses)
    client.write_multiple(S0_BASES[axis], s0)
    client.write_multiple(S1_BASES[axis], [0] * 4)


def main() -> int:
    parser = argparse.ArgumentParser(
        description="PLSR P15 四轴正负软限位边界精度测试"
    )
    parser.add_argument("--port", help="串口，例如 COM5；只有一个串口时可省略")
    parser.add_argument("--baud", type=int, default=9600)
    parser.add_argument("--slave", type=int, default=1)
    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[:5] != [
            0x504C,
            0x5352,
            0x0100,
            CONTROL_WINDOW_WORDS,
            0x0007,
        ]:
            raise RuntimeError(
                f"P15控制窗口未就绪：{header}；请烧录当前固件并复位"
            )
        print("P15 已就绪：软限位±1000000，K2保护矩阵")

        sequence = 500
        for axis, start_position, frequency, requested, expected_pulses in CASES:
            positive = requested > 0
            label = "正限位" if positive else "负限位"
            expected_position = SOFT_LIMIT if positive else -SOFT_LIMIT
            expected_error = (
                ERROR_LIMIT_POSITIVE if positive else ERROR_LIMIT_NEGATIVE
            )
            expected_reason = (
                STOP_LIMIT_POSITIVE if positive else STOP_LIMIT_NEGATIVE
            )

            response = send_command(
                client, sequence, axis, CMD_SET_POSITION, start_position
            )
            check_result(response, 4, RESULT_QUEUED, f"轴{axis} SET_POSITION")
            before = wait_command_applied(client, axis, sequence)
            sequence += 1

            write_job(client, axis, frequency, requested)
            response = send_call(
                client, sequence, axis, CALL_COMMIT, s2_set=2
            )
            check_result(response, 3, RESULT_OK, f"轴{axis} COMMIT")
            sequence += 1
            start_sequence = sequence
            response = send_call(
                client, start_sequence, axis, CALL_START, s2_set=2
            )
            check_result(response, 3, RESULT_QUEUED, f"轴{axis} START")
            sequence += 1

            running = wait_state(client, axis, {STATE_ACCEL, STATE_RUN})
            if running["counter_mode"] != 1:
                raise RuntimeError(f"轴{axis}未取得硬件计数器：{running}")
            stopped = wait_state(client, axis, {STATE_STOPPED})
            physical_delta = stopped["physical_pulses"] - before["physical_pulses"]
            expected_task = expected_pulses if positive else -expected_pulses
            checks = {
                "逻辑位置": (stopped["logical_position"], expected_position),
                "任务脉冲": (stopped["task_pulses"], expected_task),
                "物理脉冲增量": (physical_delta, expected_pulses),
            }
            bad = {
                name: values
                for name, values in checks.items()
                if abs(values[0] - values[1]) > 1
            }
            if bad:
                raise RuntimeError(f"轴{axis}{label}边界超差：{bad}；状态={stopped}")
            if (
                stopped["error"] != expected_error
                or stopped["stop_reason"] != expected_reason
            ):
                raise RuntimeError(f"轴{axis}{label}错误语义不正确：{stopped}")
            print(
                f"轴{axis} {label} {frequency:5d}Hz："
                f"输出={physical_delta}，位置={stopped['logical_position']}，PASS"
            )

            response = send_command(client, sequence, axis, CMD_RESET_ERROR)
            check_result(response, 4, RESULT_QUEUED, f"轴{axis} RESET_ERROR")
            reset = wait_command_applied(client, axis, sequence)
            sequence += 1
            if reset["state"] != STATE_IDLE or reset["error"] != 0:
                raise RuntimeError(f"轴{axis}错误复位不完整：{reset}")

        print("全部 PASS：四轴正/负软限位在±1脉冲窗口内停止，错误码及复位正确。")
        print("请核对波形：Q0/Q2约200个上升沿，Q1/Q3约1000个上升沿。")
    return 0


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