#!/usr/bin/env python3
"""
A script to parse and convert Kogger SB protocol logs into a human-readable format.
"""

import csv
import struct
import sys
import ast
import os

# Based on the Kogger SB protocol specification PDF and user-provided details.

# (From PDF page 5 and others)
ID_MAP = {
    0x01: "ID_TIMESTAMP",
    0x02: "ID_DIST",
    0x03: "ID_CHART",
    0x04: "ID_ATTITUDE",
    0x05: "ID_TEMP",
    0x10: "ID_DATASET",
    0x11: "ID_DIST_SETUP",
    0x12: "ID_CHART_SETUP",
    0x14: "ID_TRANSC",
    0x15: "ID_SND_SPD",
    0x18: "ID_UART",
    0x1B: "ID_IMU_SETUP",
    0x20: "ID_VERSION",
    0x21: "ID_MARK",
    0x22: "ID_DIAG",
    0x23: "ID_FLASH",
    0x24: "ID_BOOT",
    0x25: "ID_UPDATE",
    0x64: "ID_NAV",
    0x65: "ID_USBL_SOLUTION",
    0x66: "ID_SIGNAL_ENCODER",
    0x67: "ID_SIGNAL_DECODER",
    0x68: "ID_USBL_CONTROL",
    0x79: "ID_DVL_VEL",
    # Some IDs have aliases
    102: "ID_SIGNAL_ENCODER",
    103: "ID_SIGNAL_DECODER",
    121: "ID_DVL_VEL",
}

def parse_route(route_byte):
    """Parses the ROUTE byte."""
    dev_address = route_byte & 0b1111
    return {"dev_address": dev_address}

def parse_mode(mode_byte):
    """Parses the MODE byte."""
    type_val = mode_byte & 0b11
    type_map = {
        0: "Reserved",
        1: "CON:DEV→HST",
        2: "SET:HST→DEV",
        3: "GET:HST→DEV",
    }
    version = (mode_byte >> 3) & 0b111
    mark = (mode_byte >> 6) & 0b1
    response = (mode_byte >> 7) & 0b1
    return {
        "type": type_map.get(type_val, "Unknown"),
        "type_val": type_val,
        "version": version,
        "mark": mark,
        "response": response,
    }

def parse_payload(msg_id, mode, payload):
    """Parses the payload based on message ID, mode, and version."""
    msg_name = ID_MAP.get(msg_id)

    # Check for generic RESP message first (PDF page 6)
    if mode['type_val'] == 1 and len(payload) == 3: # CONTENT from DEVICE
        try:
            code, _check1, _check2 = struct.unpack('<BBB', payload)
            resp_codes = {
                0: "NONE", 1: "OK", 2: "ERR_CHECKSUMM", 3: "ERR_PAYLOAD",
                4: "ERR_ID", 5: "ERR_VERSION", 6: "ERR_TYPE", 7: "ERR_KEY",
                8: "ERR_RUNTIME"
            }
            if code in resp_codes:
                return f"RESP: Status={resp_codes.get(code)} ({code})"
        except struct.error:
            pass # Not a valid RESP message, fall through

    # Specific payload parsers
    if msg_name == "ID_USBL_SOLUTION" and mode['version'] == 0:
        # This is a GET request, no payload is expected.
        if mode['type_val'] == 3 and len(payload) == 0:
            return "Requesting UsblSolution"
        if len(payload) == 152:
            try:
                # Format string derived from the user-provided Python code
                fmt = '<BBHqIqfffffffffddffffddIff8f'
                sol = struct.unpack(fmt, payload)
                return (
                        f"Usbl: ID={sol[0]},"
                        f"Dist={sol[6]:.2f}m, "
                        f"Azimuth={sol[8]:.2f}deg, Elev={sol[10]:.2f}deg, "
                        f"SNR={sol[12]:.1f}")
            except struct.error as e:
                 return f"Error: Failed to unpack UsblSolution: {e}"
        else:
            return f"Error: UsblSolution payload wrong size ({len(payload)} bytes)"


    if msg_name == "ID_USBL_CONTROL":
        if mode['version'] == 1 and len(payload) == 5:
            timeout, address = struct.unpack('<IB', payload)
            return f"AcousticPing: Timeout={timeout} us, Address={address}"
        if mode['version'] == 3 and len(payload) == 4:
            timeout, = struct.unpack('<I', payload)
            return f"AutoResponseTimeout: Timeout={timeout} us"
        if mode['version'] == 4 and len(payload) == 1:
            address, = struct.unpack('<B', payload)
            return f"AutoResponseAddressFilter: Address={address}"
        if mode['version'] == 5 and len(payload) == 1:
            address, = struct.unpack('<B', payload)
            return f"AutoResponsePayloadAddress: Address={address}"

    if msg_name == "ID_TEMP" and mode['type_val'] == 1 and len(payload) == 2:
        temp_val, = struct.unpack('<h', payload)
        return f"Temperature: {temp_val * 0.01:.2f} C"

    if msg_name == "ID_DIST" and mode['type_val'] == 1:
        if mode['version'] == 0 and len(payload) == 4:
            distance, = struct.unpack('<I', payload)
            return f"Distance: {distance} mm"
        if mode['version'] == 1 and len(payload) == 6:
            _num, _strong, distance, _width = struct.unpack('<BBIH', payload)
            return f"Distance (v1): {distance} mm"

    if msg_name == "ID_ATTITUDE" and mode['type_val'] == 1:
        if mode['version'] == 0 and len(payload) == 6:
            yaw, pitch, roll = struct.unpack('<hhh', payload)
            return (
                    f"Attitude (Euler): Yaw={yaw*0.01:.2f}, "
                    f"Pitch={pitch*0.01:.2f}, Roll={roll*0.01:.2f} deg")
        if mode['version'] == 1 and len(payload) == 16:
            w0, w1, w2, w3 = struct.unpack('<ffff', payload)
            return f"Attitude (Quaternion): w0={w0}, w1={w1}, w2={w2}, w3={w3}"

    # Fallback for unknown or unhandled payloads
    return f"Payload (hex): {payload.hex()}"

def parse_message(data):
    """Parses a full binary message frame."""
    if len(data) < 8:
        return "Error: Message too short"

    try:
        sync1, sync2, route_byte, mode_byte, msg_id, length = struct.unpack('<BBBBBB', data[:6])
    except struct.error:
        return "Error: Could not unpack header"

    if sync1 != 0xBB or sync2 != 0x55:
        return "Error: Invalid sync bytes"

    header_end = 6
    payload_end = header_end + length
    checksum_end = payload_end + 2

    if len(data) != checksum_end:
        return f"Error: Length mismatch. Header says {length}, but packet is {len(data)}"

    payload = data[header_end:payload_end]
    
    route_info = parse_route(route_byte)
    mode_info = parse_mode(mode_byte)
    
    msg_name = ID_MAP.get(msg_id, f"Unknown ID (0x{msg_id:02x})")
    payload_str = parse_payload(msg_id, mode_info, payload)

    return (
            f"ID:{msg_name[3:]:<14} | "
            f"Type:{mode_info['type']:<11} | "
            f"Vr:{mode_info['version']} | "
            f"Ln:{length:<3} | "
            f"Addr:{route_info['dev_address']} | "
            f"{payload_str}"
    )

def print_save(way, timestamp, message, outfile):
    outfile.write(f"{timestamp} {way}: {message}\n")


def main(log_file):
    """Main function to read and process the log file."""
    base, ext = os.path.splitext(log_file)
    output_file = f"{base}_hum{ext}"

    print(f"Input file:  {log_file}")
    print(f"Output file: {output_file}")
    
    reassembly_buffer = b''
    last_direction = None

    try:
        with open(log_file, 'r', newline='') as infile, open(output_file, 'w') as outfile:
            reader = csv.reader(infile)
            for i, row in enumerate(reader):
                if len(row) != 3:
                    outfile.write(f"Skipping malformed row {i+1}: {row}\n")
                    continue
                
                timestamp, direction, data_str = row
                
                try:
                    data_bytes = ast.literal_eval(data_str)
                    if not isinstance(data_bytes, bytes):
                        raise TypeError("Not a bytes object")
                except (ValueError, SyntaxError, TypeError):
                    outfile.write(f"{timestamp} {direction.ljust(8)}: Error: Could not parse data literal: {data_str}\n")
                    continue

                # If the direction changes, reset the buffer.
                if direction != last_direction:
                    reassembly_buffer = b''
                last_direction = direction

                if direction == "SENT":
                    # SENT messages are assumed to be complete
                    human_readable_msg = parse_message(data_bytes)
                    print_save("TX", timestamp, human_readable_msg, outfile)
                    continue

                # Handle reassembly for RECEIVED messages
                reassembly_buffer += data_bytes
                
                while len(reassembly_buffer) >= 8: # Minimum possible message size
                    if not reassembly_buffer.startswith(b'\xbbU'):
                        # Buffer doesn't start with sync bytes, find the next sync sequence
                        sync_pos = reassembly_buffer.find(b'\xbbU')
                        if sync_pos == -1:
                            # No sync bytes, discard the whole buffer
                            reassembly_buffer = b''
                            break 
                        else:
                            # Discard garbage before the sync bytes
                            reassembly_buffer = reassembly_buffer[sync_pos:]
                    
                    if len(reassembly_buffer) < 6:
                        # Not enough data for a header
                        break

                    # Try to read the payload length
                    payload_len = reassembly_buffer[5]
                    full_msg_len = 6 + payload_len + 2 # header + payload + checksum

                    if len(reassembly_buffer) >= full_msg_len:
                        # We have a complete message
                        message_to_parse = reassembly_buffer[:full_msg_len]
                        human_readable_msg = parse_message(message_to_parse)
                        print_save("RX", timestamp, human_readable_msg, outfile)
                        
                        # Keep the rest of the buffer for the next message
                        reassembly_buffer = reassembly_buffer[full_msg_len:]
                    else:
                        # Not enough data for a full message, wait for more
                        break

    except FileNotFoundError:
        print(f"Error: File not found at {log_file}", file=sys.stderr)
        sys.exit(1)
    except Exception as e:
        print(f"An unexpected error occurred: {e}", file=sys.stderr)
        sys.exit(1)

if __name__ == "__main__":
    if len(sys.argv) != 2:
        print(f"Usage: python {sys.argv[0]} <log_file.csv>", file=sys.stderr)
        sys.exit(1)
    main(sys.argv[1])