from __future__ import annotations

import bisect
import struct

from pipeline_common import *
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments


_SPAN_CACHE: dict[int, tuple[list[RawChunk], list[int]]] = {}


def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
    key = id(chunks)
    cached = _SPAN_CACHE.get(key)
    if cached is not None and cached[0] is chunks:
        return cached[1]
    starts = []
    cursor = 0
    for chunk in chunks:
        starts.append(cursor)
        cursor += len(chunk.raw)
    _SPAN_CACHE[key] = (chunks, starts)
    return starts


def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: int) -> dict:
    starts = _chunk_starts(chunks)
    start_index = max(0, min(len(chunks) - 1, bisect.bisect_right(starts, start) - 1))
    end_index = max(start_index, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
    start_chunk = chunks[start_index]
    end_chunk = chunks[end_index]
    return {
        "source_segment_id": segment_id,
        "source_chunk_sequence_first": start_chunk.sequence,
        "source_chunk_sequence_last": end_chunk.sequence,
        "source_raw_file_offset": start_chunk.raw_file_offset + (start - starts[start_index]),
        "source_raw_byte_length": end - start,
        "host_receive_utc_ns": ticks_to_unix_ns(end_chunk.receive_utc_ticks),
        "host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
    }


def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
    rows = []
    for segment_id, chunks in iter_contiguous_segments(capture.chunks):
        stream = b"".join(chunk.raw for chunk in chunks)
        cursor = 0
        while cursor < len(stream):
            newline = stream.find(b"\n", cursor)
            if newline < 0:
                break
            end = newline + 1
            raw_line = stream[cursor:end].rstrip(b"\r\n")
            start = cursor
            cursor = end
            if not raw_line:
                continue
            line = raw_line.decode("ascii", "replace")
            row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
            row.update(source_for_span(chunks, start, end, segment_id))
            try:
                if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
                    row.update(parse_gga(line))
                elif line.startswith("#PVTSLNA"):
                    row.update(parse_pvtslna(line))
                elif line.startswith("#UNIHEADINGA"):
                    row.update(parse_heading(line))
            except ValueError as ex:
                row["parse_error"] = str(ex)
            rows.append(row)
    return rows


def crc8_fdilink(data: bytes) -> int:
    crc = 0
    for value in data:
        crc ^= value
        for _ in range(8):
            crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
    return crc


def crc16_fdilink(data: bytes) -> int:
    crc = 0
    for value in data:
        crc ^= value << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
    return crc


def parse_n300_imu_capture(capture: CaptureFile) -> list[dict]:
    """Parse Wheeltec N300 FDILink IMU frames; normalize to HI13-like keys."""

    rows = []
    expected_lengths = {0x40: 56, 0x41: 48}
    for segment_id, chunks in iter_contiguous_segments(capture.chunks):
        stream = b"".join(chunk.raw for chunk in chunks)
        cursor = 0
        while cursor < len(stream):
            start = stream.find(b"\xFC", cursor)
            if start < 0:
                break
            if start + 8 > len(stream):
                break
            payload_length = stream[start + 2]
            end = start + payload_length + 8
            if end > len(stream):
                if stream.find(b"\xFC", start + 1) < 0:
                    break
                cursor = start + 1
                continue
            frame = stream[start:end]
            if frame[-1] != 0xFD:
                cursor = start + 1
                continue
            packet_id = frame[1]
            payload = frame[7:-1]
            header_ok = crc8_fdilink(frame[:4]) == frame[4]
            payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big")
            expected = expected_lengths.get(packet_id)
            length_ok = expected is None or len(payload) == expected
            row = {
                "type": "N300",
                "tag": int(packet_id),
                "frame_length": len(frame),
                "crc_valid": bool(header_ok and payload_ok and length_ok),
                "raw_frame_hex": frame.hex(),
            }
            row.update(source_for_span(chunks, start, end, segment_id))
            if row["crc_valid"] and packet_id == 0x40:
                try:
                    gyro = struct.unpack_from("<3f", payload, 0)
                    accel = struct.unpack_from("<3f", payload, 12)
                    device_us = struct.unpack_from("<q", payload, 48)[0]
                    row.update(
                        {
                            "device_timestamp_us": int(device_us),
                            # build_multisensor_npz.estimate_imu_times uses ms.
                            "device_timestamp_ms": int(device_us) // 1000,
                            "gyro_x_radps": gyro[0],
                            "gyro_y_radps": gyro[1],
                            "gyro_z_radps": gyro[2],
                            "accel_x_mps2": accel[0],
                            "accel_y_mps2": accel[1],
                            "accel_z_mps2": accel[2],
                            "pps_sync_stamp_ms": -1,
                        }
                    )
                except (IndexError, struct.error, ValueError) as ex:
                    row["parse_error"] = str(ex)
                    row["crc_valid"] = False
            elif row["crc_valid"] and packet_id == 0x41:
                try:
                    device_us = struct.unpack_from("<q", payload, 40)[0]
                    row.update(
                        {
                            "device_timestamp_us": int(device_us),
                            "device_timestamp_ms": int(device_us) // 1000,
                            "pps_sync_stamp_ms": -1,
                        }
                    )
                except (IndexError, struct.error, ValueError) as ex:
                    row["parse_error"] = str(ex)
            rows.append(row)
            cursor = end if row["crc_valid"] else start + 1
    return rows


def parse_hi13_imu_capture(capture: CaptureFile) -> list[dict]:
    rows = []
    for segment_id, chunks in iter_contiguous_segments(capture.chunks):
        stream = b"".join(chunk.raw for chunk in chunks)
        cursor = 0
        while True:
            start = stream.find(b"\x5a\xa5", cursor)
            if start < 0 or start + 6 > len(stream):
                break
            payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
            frame_length = 6 + payload_length
            if payload_length <= 0 or payload_length > 512:
                cursor = start + 1
                continue
            if start + frame_length > len(stream):
                break
            frame = stream[start:start + frame_length]
            expected = int.from_bytes(frame[4:6], "little")
            actual = crc16_hi13(frame[:4] + frame[6:])
            end = start + frame_length
            row = {
                "type": "HI13",
                "tag": frame[6],
                "frame_length": frame_length,
                "crc_valid": expected == actual,
                "raw_frame_hex": frame.hex(),
            }
            row.update(source_for_span(chunks, start, end, segment_id))
            if row["crc_valid"]:
                try:
                    row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
                except (IndexError, struct.error, ValueError) as ex:
                    row["parse_error"] = str(ex)
            rows.append(row)
            cursor = end
    return rows


def parse_imu_capture(capture: CaptureFile) -> list[dict]:
    """Prefer N300 FDILink when present; fall back to legacy HI13."""

    n300 = parse_n300_imu_capture(capture)
    if any(row.get("crc_valid") and row.get("type") == "N300" for row in n300):
        return n300
    return parse_hi13_imu_capture(capture)