packet_parser.py

"""DMA-03B の NOW 応答・測定パケットの解析。

DMA-03B は 3ch デジタルアンプ。測定では **有効チャンネル分のみ** が送られるため、
パケット長は「有効ch数（enabled_ch_count）」に依存して可変になる。

測定パケットのフォーマット（デバイス仕様）::

    [0xAA × 4]                                  ヘッダ
    [有効ch × 符号付き int16 LE] × まとめ数       ボディ
    [0x55 × 4]                                  フッタ

    packet_size = 4 + enabled_ch * 2 * matome + 4

* ボディの値は **デバイス側で干渉補正・フルスケール換算済み** の符号付き 16bit
  （±32000 = フルスケールへ正規化されている）。したがって
  ``eng = value * FS / 32000`` で物理量（N など）が得られる。
* NOW 応答は **生 ADC 値**（補正前）で、有効ch × int16 LE（ヘッダ／フッタ無し）。
  IDLE 以外のステータスでは全 0 のダミーが返る。
* ロボット用モードでは 1 サンプルを「有効ch × 4 桁の大文字 16 進」+ LF で送る。
  こちらの値も測定同様にデバイス側で補正済み。
"""

from __future__ import annotations

import numpy as np

# 定数
CH_COUNT = 3                     # 物理チャンネル数（CH1, CH2, CH3）
HEADER = b"\xAA\xAA\xAA\xAA"
FOOTER = b"\x55\x55\x55\x55"
HEADER_SIZE = len(HEADER)
FOOTER_SIZE = len(FOOTER)

# 工学値変換の満点（DMA-03B のファーム ToEng / サンプルソフトに準拠：32000 がフルスケール）
FULL_SCALE_COUNT = 32000.0


class PacketError(Exception):
    """パケット解析に関するエラー。"""


def ad_to_eng(ad, full_scale) -> np.ndarray:
    """測定値（補正済み AD 値）を工学値（物理量）へ変換する。

    DMA-03B 標準ソフトの換算式に準拠::

        eng = ad * full_scale / 32000

    Parameters
    ----------
    ad:
        ``(..., C)`` 形状の測定値（符号付き int16 想定）。``C`` は列数で、
        測定では有効ch数に一致する。
    full_scale:
        各列（チャンネル）に対応するフルスケール値。要素数は ``ad`` の
        最終次元と一致する必要がある。
    """
    fs = np.asarray(full_scale, dtype=float)
    arr = np.asarray(ad, dtype=float)
    if fs.shape[-1] != arr.shape[-1]:
        raise ValueError(
            f"full_scale の要素数 {fs.shape[-1]} が data の列数 {arr.shape[-1]} と一致しません。"
        )
    return arr * fs / FULL_SCALE_COUNT


def now_size(enabled_ch_count: int) -> int:
    """NOW 応答のバイト数（有効ch × int16）を返す。"""
    return enabled_ch_count * 2


def body_size(matome: int, enabled_ch_count: int) -> int:
    """ボディ（AD 値部）のバイト数を求める。"""
    return enabled_ch_count * 2 * matome


def packet_size(matome: int, enabled_ch_count: int) -> int:
    """まとめ数・有効ch数からパケット全体のバイト数を求める。"""
    return HEADER_SIZE + body_size(matome, enabled_ch_count) + FOOTER_SIZE


def parse_now(raw: bytes, enabled_ch_count: int) -> np.ndarray:
    """NOW 応答を ``(enabled_ch_count,)`` の符号付き int16 配列に変換する。

    NOW は **生 ADC 値**（干渉補正前）である点に注意。
    """
    expected = now_size(enabled_ch_count)
    if len(raw) != expected:
        raise PacketError(f"NOW データ長が不正です: {len(raw)} (期待値 {expected})")
    return np.frombuffer(raw, dtype="<i2").copy()


def parse_robot_hex(line: str, enabled_ch_count: int) -> np.ndarray:
    """ロボットモードの 1 サンプル（16進 ASCII）を ``(enabled_ch_count,)`` int16 で返す。

    有効な各チャンネル（符号付き 16bit）が上位ニブルから 4 桁の大文字 16 進で
    出力される。1 サンプル = 有効ch数 × 4 文字。
    """
    line = line.strip()
    expected = enabled_ch_count * 4
    if len(line) != expected:
        raise PacketError(
            f"ロボットサンプル長が不正です: {len(line)} (期待値 {expected})"
        )
    values = []
    for i in range(enabled_ch_count):
        word = int(line[i * 4:i * 4 + 4], 16)
        if word >= 0x8000:  # 2 の補数で符号付き化
            word -= 0x10000
        values.append(word)
    return np.array(values, dtype=np.int16)


def parse_packet(raw: bytes, matome: int, enabled_ch_count: int) -> np.ndarray:
    """1 パケットを ``(matome, enabled_ch_count)`` の符号付き int16 配列に変換する。

    ヘッダ／フッタを検証する。``raw`` はちょうど 1 パケット分であること。
    """
    expected = packet_size(matome, enabled_ch_count)
    if len(raw) != expected:
        raise PacketError(f"パケット長が不正です: {len(raw)} (期待値 {expected})")
    if raw[:HEADER_SIZE] != HEADER:
        raise PacketError(f"ヘッダ不正: {raw[:HEADER_SIZE].hex()}")
    if raw[-FOOTER_SIZE:] != FOOTER:
        raise PacketError(f"フッタ不正: {raw[-FOOTER_SIZE:].hex()}")

    body = raw[HEADER_SIZE:HEADER_SIZE + body_size(matome, enabled_ch_count)]
    values = np.frombuffer(body, dtype="<i2")
    return values.reshape(matome, enabled_ch_count).copy()


def find_packet(buffer: bytes, matome: int, enabled_ch_count: int):
    """バイト列の先頭からヘッダを探し、見つかれば 1 パケットを解析する。

    Returns
    -------
    (data, consumed):
        ``data`` は解析できたパケット（``None`` ならまだ揃っていない）。
        ``consumed`` は ``buffer`` から消費してよいバイト数。
    """
    size = packet_size(matome, enabled_ch_count)
    idx = buffer.find(HEADER)
    if idx < 0:
        # ヘッダが無い。末尾の途中ヘッダ可能性分だけ残して破棄。
        keep = HEADER_SIZE - 1
        return None, max(0, len(buffer) - keep)
    if len(buffer) - idx < size:
        # ヘッダ位置以降のゴミだけ捨てる
        return None, idx
    raw = buffer[idx:idx + size]
    data = parse_packet(raw, matome, enabled_ch_count)
    return data, idx + size