"""STS protocol frame builders/parsers."""

from __future__ import annotations

from .constants import BROADCAST_ID, HEADER, Instruction, MemAddr


def calculate_checksum(data: list[int]) -> int:
    """Compute STS checksum from bytes excluding frame header."""
    return (~sum(data)) & 0xFF


def int16_to_bytes(value: int) -> list[int]:
    """Convert 16-bit value to little-endian [low, high]."""
    if value < 0:
        value &= 0xFFFF
    return [value & 0xFF, (value >> 8) & 0xFF]


def bytes_to_int16(low: int, high: int, signed: bool = False) -> int:
    """Convert little-endian bytes to 16-bit integer."""
    value = low | (high << 8)
    if signed and value >= 0x8000:
        return value - 0x10000
    return value


def _validate_sts_position_mode_word(name: str, value: int) -> None:
    """Validate 16-bit position-mode argument.

    In current pymyarm position mode, signed-magnitude STS encoding
    is not exposed. Reject negative values explicitly to avoid
    accidentally sending two's-complement data.
    """
    if value < 0:
        raise ValueError(
            f"{name} must be non-negative in current STS position mode; "
            "negative values require STS signed-magnitude encoding support."
        )


def build_instruction_frame(
    servo_id: int, instruction: int, params: list[int] | None = None
) -> bytes:
    """Build generic instruction frame."""
    if params is None:
        params = []
    length = len(params) + 2
    payload = [servo_id, length, instruction] + params
    checksum = calculate_checksum(payload)
    return bytes([HEADER, HEADER] + payload + [checksum])


def build_ping_frame(servo_id: int) -> bytes:
    """Build PING frame."""
    return build_instruction_frame(servo_id, Instruction.PING)


def build_read_frame(servo_id: int, start_addr: int, read_len: int) -> bytes:
    """Build READ_DATA frame."""
    return build_instruction_frame(
        servo_id, Instruction.READ_DATA, [start_addr, read_len]
    )


def build_write_frame(servo_id: int, start_addr: int, data: list[int]) -> bytes:
    """Build WRITE_DATA frame."""
    return build_instruction_frame(
        servo_id, Instruction.WRITE_DATA, [start_addr] + data
    )


def build_position_calibrate_frame(servo_id: int, target_position: int) -> bytes:
    """Build POSITION_CALIBRATE (0x0B) frame.

    Instructs the servo to compute and store the offset needed to map its
    current physical position to *target_position*. Writes to EEPROM
    internally — no LOCK_FLAG management required.

    TX: [0xFF, 0xFF, servo_id, 0x04, 0x0B, pos_low, pos_high, checksum]
    """
    return build_instruction_frame(
        servo_id, Instruction.POSITION_CALIBRATE, int16_to_bytes(target_position)
    )


def build_sync_write_frame(
    start_addr: int, data_len: int, servo_data: list[tuple[int, list[int]]]
) -> bytes:
    """Build SYNC_WRITE frame for multiple servos."""
    params: list[int] = [start_addr, data_len]
    for servo_id, data in servo_data:
        params.append(servo_id)
        params.extend(data)
    return build_instruction_frame(BROADCAST_ID, Instruction.SYNC_WRITE, params)


def build_sync_read_frame(
    start_addr: int, data_len: int, servo_ids: list[int]
) -> bytes:
    """Build SYNC_READ frame."""
    params = [start_addr, data_len] + servo_ids
    return build_instruction_frame(BROADCAST_ID, Instruction.SYNC_READ, params)


def build_sync_position_frame(
    servo_positions: list[tuple[int, int]],
    speed: int,
    acceleration: int = 0,
) -> bytes:
    """Build synced position frame writing ACC..GOAL_SPEED region."""
    _validate_sts_position_mode_word("speed", speed)
    servo_data: list[tuple[int, list[int]]] = []
    for servo_id, position in servo_positions:
        _validate_sts_position_mode_word("position", position)
        data = (
            [acceleration & 0xFF]
            + int16_to_bytes(position)
            + [0, 0]
            + int16_to_bytes(speed)
        )
        servo_data.append((servo_id, data))
    return build_sync_write_frame(MemAddr.ACCELERATION, 7, servo_data)


def parse_response_frame(frame: bytes) -> tuple[int, int, list[int]] | None:
    """Parse one response frame into (servo_id, error_code, params)."""
    if len(frame) < 6:
        return None

    header_pos = -1
    for i in range(len(frame) - 1):
        if frame[i] == HEADER and frame[i + 1] == HEADER:
            header_pos = i
            break
    if header_pos < 0:
        return None

    if header_pos + 6 > len(frame):
        return None

    servo_id = frame[header_pos + 2]
    length = frame[header_pos + 3]
    error_code = frame[header_pos + 4]
    param_len = length - 2

    param_start = header_pos + 5
    param_end = param_start + param_len
    checksum_pos = header_pos + 3 + length

    if checksum_pos >= len(frame) or param_end > len(frame):
        return None

    params = list(frame[param_start:param_end])
    expected = frame[checksum_pos]
    actual = calculate_checksum(list(frame[header_pos + 2 : checksum_pos]))
    if expected != actual:
        return None

    return servo_id, error_code, params


def parse_sync_read_responses(
    raw: bytes, servo_ids: list[int], data_len: int
) -> dict[int, list[int] | None]:
    """Parse concatenated sync-read responses into per-servo payload dict."""
    payloads, _ = parse_sync_read_responses_with_errors(raw, servo_ids, data_len)
    return payloads


def parse_sync_read_responses_with_errors(
    raw: bytes,
    servo_ids: list[int],
    data_len: int,
) -> tuple[dict[int, list[int] | None], dict[int, int | None]]:
    """Parse sync-read responses into payload and error-code dictionaries."""
    parsed: dict[int, list[int] | None] = {}
    errors: dict[int, int | None] = {}
    pos = 0

    while pos < len(raw) - 1:
        if raw[pos] != HEADER or raw[pos + 1] != HEADER:
            pos += 1
            continue

        if pos + 6 > len(raw):
            break

        servo_id = raw[pos + 2]
        length = raw[pos + 3]
        frame_end = pos + 3 + length
        if frame_end >= len(raw):
            break

        frame = raw[pos : frame_end + 1]
        decoded = parse_response_frame(frame)
        if decoded is not None:
            _, error_code, params = decoded
            errors[servo_id] = error_code
            parsed[servo_id] = params[:data_len]

        pos = frame_end + 1

    # Ensure requested IDs exist in map for caller simplicity.
    for servo_id in servo_ids:
        parsed.setdefault(servo_id, None)
        errors.setdefault(servo_id, None)
    return parsed, errors
