packet_parser.py

"""
IMS-SD パケットデータ解析モジュール
"""

import struct
from typing import List, Optional, Tuple, Dict
from dataclasses import dataclass


@dataclass
class PacketData:
    """パケットデータクラス（1まとめ分）"""
    soc: int               # 充電残量（SOC）
    temperature: float      # 温度 [℃]
    samples: List['SampleData']  # サンプルデータリスト


@dataclass
class SampleData:
    """1サンプル分のセンサーデータ（生データ）"""
    acc_x: int   # 加速度X軸（生データ）
    acc_y: int   # 加速度Y軸（生データ）
    acc_z: int   # 加速度Z軸（生データ）
    gyro_x: int  # ジャイロX軸（生データ）
    gyro_y: int  # ジャイロY軸（生データ）
    gyro_z: int  # ジャイロZ軸（生データ）
    mag_x: int   # 地磁気X軸（生データ）
    mag_y: int   # 地磁気Y軸（生データ）
    mag_z: int   # 地磁気Z軸（生データ）


@dataclass
class EngineeringData:
    """工学値変換済みセンサーデータ"""
    timestamp: float     # タイムスタンプ
    acc_x: float         # 加速度X軸 [G]
    acc_y: float         # 加速度Y軸 [G]
    acc_z: float         # 加速度Z軸 [G]
    gyro_x: float        # ジャイロX軸 [deg/s]
    gyro_y: float        # ジャイロY軸 [deg/s]
    gyro_z: float        # ジャイロZ軸 [deg/s]
    mag_x: float         # 地磁気X軸 [uT]
    mag_y: float         # 地磁気Y軸 [uT]
    mag_z: float         # 地磁気Z軸 [uT]
    soc: int             # 充電残量（SOC）
    temperature: float   # 温度 [℃]


class PacketParser:
    """パケットデータ解析クラス"""

    def __init__(self, device, ch_count: int = 9, matome_pc: int = 10):
        """
        初期化

        Args:
            device: ImsDeviceインスタンス（校正値取得用）
            ch_count: チャンネル数（9軸センサー）
            matome_pc: PCまとめ数
        """
        self.device = device
        self.ch_count = ch_count
        self.matome_pc = matome_pc

        # パケットサイズ計算
        self.packet_header_size = 4
        self.packet_footer_size = 4
        self.packet_data_size = ch_count * 2 * matome_pc + 2 * 2  # データ部分のサイズ
        self.total_packet_size = (self.packet_data_size +
                                   self.packet_header_size +
                                   self.packet_footer_size)

    def parse_packet(self, packet_bytes: bytes) -> Optional[PacketData]:
        """
        パケットバイト列を解析

        Args:
            packet_bytes: パケットバイト列（192バイト）

        Returns:
            PacketData: 解析されたパケットデータ、エラー時はNone
        """
        if len(packet_bytes) != self.total_packet_size:
            print(f"パケットサイズエラー: {len(packet_bytes)} != {self.total_packet_size}")
            return None

        try:
            # ヘッダー（4バイト）がすべて0xAAであることを確認
            header = packet_bytes[0:self.packet_header_size]
            if header != b'\xAA\xAA\xAA\xAA':
                print(f"ヘッダーエラー: {header.hex()} != AAAAAAAA")
                return None

            offset = self.packet_header_size

            # 温度を取得（2バイト、リトルエンディアン、符号付き）
            temperature_raw = struct.unpack('<h', packet_bytes[offset:offset+2])[0]
            offset += 2

            # 充電残量（SOC）を取得（2バイト、リトルエンディアン、符号なし）
            soc_raw = struct.unpack('<H', packet_bytes[offset:offset+2])[0]
            offset += 2

            # 実際の値に変換
            temperature = self._convert_temperature(temperature_raw)
            soc = soc_raw  # AD値がそのままSOC

            # サンプルデータを解析
            samples = []
            for i in range(self.matome_pc):
                sample = self._parse_sample(packet_bytes, offset)
                if sample:
                    samples.append(sample)
                offset += self.ch_count * 2  # 各チャンネル2バイト

            # フッター（4バイト）がすべて0x55であることを確認
            footer_start = self.total_packet_size - self.packet_footer_size
            footer = packet_bytes[footer_start:self.total_packet_size]
            if footer != b'\x55\x55\x55\x55':
                print(f"フッターエラー: {footer.hex()} != 55555555")
                return None

            return PacketData(
                soc=soc,
                temperature=temperature,
                samples=samples
            )

        except Exception as e:
            print(f"パケット解析エラー: {e}")
            return None

    def _parse_sample(self, packet_bytes: bytes, offset: int) -> Optional[SampleData]:
        """
        1サンプル分のデータを解析

        Args:
            packet_bytes: パケットバイト列
            offset: 読み込み開始位置

        Returns:
            SampleData: サンプルデータ
        """
        try:
            # データの順番は Z, X, Y（リトルエンディアン、符号付き16bit）
            # 加速度
            acc_z = struct.unpack('<h', packet_bytes[offset:offset+2])[0]
            acc_x = struct.unpack('<h', packet_bytes[offset+2:offset+4])[0]
            acc_y = struct.unpack('<h', packet_bytes[offset+4:offset+6])[0]

            # ジャイロ
            gyro_z = struct.unpack('<h', packet_bytes[offset+6:offset+8])[0]
            gyro_x = struct.unpack('<h', packet_bytes[offset+8:offset+10])[0]
            gyro_y = struct.unpack('<h', packet_bytes[offset+10:offset+12])[0]

            # 地磁気
            mag_z = struct.unpack('<h', packet_bytes[offset+12:offset+14])[0]
            mag_x = struct.unpack('<h', packet_bytes[offset+14:offset+16])[0]
            mag_y = struct.unpack('<h', packet_bytes[offset+16:offset+18])[0]

            # 正負反転（Z軸とY軸）
            acc_z *= -1
            acc_y *= -1
            gyro_z *= -1
            gyro_y *= -1

            return SampleData(
                acc_x=acc_x,
                acc_y=acc_y,
                acc_z=acc_z,
                gyro_x=gyro_x,
                gyro_y=gyro_y,
                gyro_z=gyro_z,
                mag_x=mag_x,
                mag_y=mag_y,
                mag_z=mag_z
            )

        except Exception as e:
            print(f"サンプル解析エラー: {e}")
            return None

    def parse_and_convert(self, packet_bytes: bytes, packet_count: int) -> List[EngineeringData]:
        """
        パケットを解析して工学値変換済みデータリストを返す

        Args:
            packet_bytes: パケットバイト列
            packet_count: パケット番号（タイムスタンプ計算用）

        Returns:
            List[EngineeringData]: 工学値変換済みデータリスト
        """
        engineering_data_list = []

        # パケット解析
        packet_data = self.parse_packet(packet_bytes)
        if not packet_data:
            return engineering_data_list

        # 各サンプルを工学値変換
        for i, sample in enumerate(packet_data.samples):
            # 3軸同時変換
            acc_x, acc_y, acc_z = self._convert_acc_value(sample.acc_x, sample.acc_y, sample.acc_z)
            gyro_x, gyro_y, gyro_z = self._convert_gyro_value(sample.gyro_x, sample.gyro_y, sample.gyro_z)
            mag_x, mag_y, mag_z = self._convert_mag_value(sample.mag_x, sample.mag_y, sample.mag_z)

            eng_data = EngineeringData(
                timestamp=((packet_count - 1) * self.matome_pc + i) / 100.0,  # 100Hzサンプリングなので100で割る
                acc_x=acc_x,
                acc_y=acc_y,
                acc_z=acc_z,
                gyro_x=gyro_x,
                gyro_y=gyro_y,
                gyro_z=gyro_z,
                mag_x=mag_x,
                mag_y=mag_y,
                mag_z=mag_z,
                soc=packet_data.soc,
                temperature=packet_data.temperature
            )
            engineering_data_list.append(eng_data)

        return engineering_data_list

    def _convert_acc_value(self, acc_x: int, acc_y: int, acc_z: int) -> Tuple[float, float, float]:
        """加速度値工学値変換（3軸同時、干渉補正含む）"""
        try:
            # 校正値取得
            range_key = self.device.acc_range
            prf_x = self.device.acc_prf.get(range_key, {}).get('X', 1.0)
            prf_y = self.device.acc_prf.get(range_key, {}).get('Y', 1.0)
            prf_z = self.device.acc_prf.get(range_key, {}).get('Z', 1.0)

            zero_x = self.device.acc_zero.get(range_key, {}).get('X', 0.0)
            zero_y = self.device.acc_zero.get(range_key, {}).get('Y', 0.0)
            zero_z = self.device.acc_zero.get(range_key, {}).get('Z', 0.0)

            # 基本変換: (Raw - Zero) * PRF
            ex = (acc_x - zero_x) * prf_x
            ey = (acc_y - zero_y) * prf_y
            ez = (acc_z - zero_z) * prf_z

            # 干渉補正
            itf = self.device.acc_itf.get(range_key, [1,0,0,0,1,0,0,0,1])
            result_x = itf[0] * ex + itf[1] * ey + itf[2] * ez
            result_y = itf[3] * ex + itf[4] * ey + itf[5] * ez
            result_z = itf[6] * ex + itf[7] * ey + itf[8] * ez

            return (result_x, result_y, result_z)
        except:
            return (float(acc_x), float(acc_y), float(acc_z))

    def _convert_gyro_value(self, gyro_x: int, gyro_y: int, gyro_z: int) -> Tuple[float, float, float]:
        """ジャイロ値工学値変換（3軸同時、干渉補正含む）"""
        try:
            # 校正値取得
            range_key = self.device.gyro_range
            prf_x = self.device.gyro_prf.get(range_key, {}).get('X', 1.0)
            prf_y = self.device.gyro_prf.get(range_key, {}).get('Y', 1.0)
            prf_z = self.device.gyro_prf.get(range_key, {}).get('Z', 1.0)

            zero_x = self.device.gyro_zero.get(range_key, {}).get('X', 0.0)
            zero_y = self.device.gyro_zero.get(range_key, {}).get('Y', 0.0)
            zero_z = self.device.gyro_zero.get(range_key, {}).get('Z', 0.0)

            # 基本変換: (Raw - Zero) * PRF
            ex = (gyro_x - zero_x) * prf_x
            ey = (gyro_y - zero_y) * prf_y
            ez = (gyro_z - zero_z) * prf_z

            # 干渉補正
            itf = self.device.gyro_itf.get(range_key, [1,0,0,0,1,0,0,0,1])
            result_x = itf[0] * ex + itf[1] * ey + itf[2] * ez
            result_y = itf[3] * ex + itf[4] * ey + itf[5] * ez
            result_z = itf[6] * ex + itf[7] * ey + itf[8] * ez

            return (result_x, result_y, result_z)
        except:
            return (float(gyro_x), float(gyro_y), float(gyro_z))

    def _convert_mag_value(self, mag_x: int, mag_y: int, mag_z: int) -> Tuple[float, float, float]:
        """地磁気値工学値変換（HighEndモデル用）"""
        try:
            # 地磁気センサー校正値取得
            off = [self.device.mag_off.get('X', 0),
                   self.device.mag_off.get('Y', 0),
                   self.device.mag_off.get('Z', 0)]

            fineoutput = [self.device.mag_fineout.get('X', 0),
                         self.device.mag_fineout.get('Y', 0),
                         self.device.mag_fineout.get('Z', 0)]

            offzero = [self.device.mag_offzero.get('X', 0),
                      self.device.mag_offzero.get('Y', 0),
                      self.device.mag_offzero.get('Z', 0)]

            sens = [self.device.mag_sens.get('X', 1),
                   self.device.mag_sens.get('Y', 1),
                   self.device.mag_sens.get('Z', 1)]

            # センサー感度チェック
            for s in sens:
                if s == 0:
                    return (0.0, 0.0, 0.0)

            # AD -> mGauss
            mag_data_x = ( fineoutput[0] * (offzero[0] - off[0]) + mag_x) / sens[0] * 1000
            mag_data_y = (-fineoutput[1] * (offzero[1] - off[1]) + mag_y) / sens[1] * 1000
            mag_data_z = (-fineoutput[2] * (offzero[2] - off[2]) + mag_z) / sens[2] * 1000

            # 干渉補正計算 - AMIGainPara enum順序: [XY, XZ, YX, YZ, ZX, ZY]
            gainpara = self.device.mag_gainpara
            if len(gainpara) >= 6:
                mag_x_corrected = ( mag_data_x
                                  - mag_data_y / sens[1] * (gainpara[2] - 127) * sens[0] / 1000  # YX
                                  - mag_data_z / sens[2] * (gainpara[4] - 127) * sens[0] / 1000) # ZX

                mag_y_corrected = ( mag_data_y
                                  - mag_data_x / sens[0] * (gainpara[0] - 127) * sens[1] / 1000  # XY
                                  - mag_data_z / sens[2] * (gainpara[5] - 127) * sens[1] / 1000) # ZY

                mag_z_corrected = ( mag_data_z
                                  - mag_data_x / sens[0] * (gainpara[1] - 127) * sens[2] / 1000  # XZ
                                  - mag_data_y / sens[1] * (gainpara[3] - 127) * sens[2] / 1000) # YZ
            else:
                # 校正データ不足時はそのまま
                mag_x_corrected = mag_data_x
                mag_y_corrected = mag_data_y
                mag_z_corrected = mag_data_z

            # 最終補正（mGauss -> uT）
            prf_x = self.device.mag_prf.get('X', 1.0)
            prf_y = self.device.mag_prf.get('Y', 1.0)
            prf_z = self.device.mag_prf.get('Z', 1.0)

            zero_x = self.device.mag_zero.get('X', 0.0)
            zero_y = self.device.mag_zero.get('Y', 0.0)
            zero_z = self.device.mag_zero.get('Z', 0.0)

            # mGauss -> uT変換 (/10) + 校正値適用
            result_x = (mag_x_corrected / 10) * prf_x + zero_x
            result_y = (mag_y_corrected / 10) * prf_y + zero_y
            result_z = (mag_z_corrected / 10) * prf_z + zero_z

            return (result_x, result_y, result_z)
        except:
            return (float(mag_x), float(mag_y), float(mag_z))


    def _convert_temperature(self, raw_value: int) -> float:
        """
        温度の生データを実際の温度値に変換（HighEndモデル用）

        Args:
            raw_value: 生データ値

        Returns:
            float: 温度 [℃]
        """
        # HighEndモデルの変換式
        return raw_value / 333.87 + 21