"""
Verify the new parser offsets on the raw captured stream.
New mappings:
- HR: 989-byte packet, offset 904 (u16 LE)
- SpO2: 286-byte packet, offset 264 (u16 LE)
- Temperature: 56-byte packet, offset 8 (float32)
- NIBP: 45-byte packet, offset 15, 17, 19 (u16 LE)
"""
import struct

def read_u16_le(data, offset):
    if offset + 2 > len(data):
        return None
    return struct.unpack_from('<H', data, offset)[0]

def read_f32_le(data, offset):
    if offset + 4 > len(data):
        return None
    return struct.unpack_from('<f', data, offset)[0]

with open("/home/prathiyuman/Prathiyuman/ContecMonitor/fiveparaminte-main/scratch/raw_contec_stream.bin", "rb") as f:
    stream = f.read()

# Parse packets
i = 0
pkts = []
while i < len(stream) - 8:
    if (stream[i + 2] == 0x04 or stream[i + 2] == 0x01) and stream[i + 3] == 0x46:
        pkt_len = struct.unpack_from('<H', stream, i)[0]
        if 30 < pkt_len < 1050 and i + pkt_len <= len(stream):
            pkts.append((pkt_len, stream[i:i + pkt_len]))
        i += max(pkt_len, 4)
    else:
        i += 1

vitals_cache = {
    "heart_rate": None,
    "spo2": None,
    "temperature": None,
    "systolic_bp": None,
    "diastolic_bp": None,
    "map_bp": None,
}

last_printed = {}

for idx, (pkt_len, pkt) in enumerate(pkts):
    changed = False
    if pkt_len == 989:
        hr = read_u16_le(pkt, 904)
        # 65535 or 9999 means disconnected
        if hr is not None and hr != 65535 and hr != 9999 and 20 <= hr <= 300:
            if vitals_cache["heart_rate"] != float(hr):
                vitals_cache["heart_rate"] = float(hr)
                changed = True
        elif hr == 65535 or hr == 9999:
            if vitals_cache["heart_rate"] is not None:
                vitals_cache["heart_rate"] = None
                changed = True
    elif pkt_len == 286:
        spo2 = read_u16_le(pkt, 264)
        if spo2 is not None and spo2 != 65535 and spo2 != 255 and 50 <= spo2 <= 100:
            if vitals_cache["spo2"] != float(spo2):
                vitals_cache["spo2"] = float(spo2)
                changed = True
        elif spo2 == 65535 or spo2 == 255:
            if vitals_cache["spo2"] is not None:
                vitals_cache["spo2"] = None
                changed = True
    elif pkt_len == 56:
        temp = read_f32_le(pkt, 8)
        if temp is not None and abs(temp - 9999.0) > 1.0 and 30.0 < temp < 45.0:
            if vitals_cache["temperature"] != round(temp, 1):
                vitals_cache["temperature"] = round(temp, 1)
                changed = True
        elif temp is not None and abs(temp - 9999.0) <= 1.0:
            if vitals_cache["temperature"] is not None:
                vitals_cache["temperature"] = None
                changed = True
    elif pkt_len == 45:
        sys_bp = read_u16_le(pkt, 15)
        dia_bp = read_u16_le(pkt, 17)
        map_bp = read_u16_le(pkt, 19)
        
        # 9999 means no NIBP measurement active
        if sys_bp is not None and sys_bp != 9999 and sys_bp > 0:
            vitals_cache["systolic_bp"] = float(sys_bp)
            changed = True
        elif sys_bp == 9999 or sys_bp == 0:
            if vitals_cache["systolic_bp"] is not None:
                vitals_cache["systolic_bp"] = None
                changed = True
                
        if dia_bp is not None and dia_bp != 9999 and dia_bp > 0:
            vitals_cache["diastolic_bp"] = float(dia_bp)
            changed = True
        elif dia_bp == 9999 or dia_bp == 0:
            if vitals_cache["diastolic_bp"] is not None:
                vitals_cache["diastolic_bp"] = None
                changed = True
                
        if map_bp is not None and map_bp != 9999 and map_bp > 0:
            vitals_cache["map_bp"] = float(map_bp)
            changed = True
        elif map_bp == 9999 or map_bp == 0:
            if vitals_cache["map_bp"] is not None:
                vitals_cache["map_bp"] = None
                changed = True

    if changed:
        current_vitals = {k: v for k, v in vitals_cache.items()}
        if current_vitals != last_printed:
            print(f"Pkt #{idx} (len={pkt_len}) -> Vitals: {current_vitals}")
            last_printed = current_vitals