#! /usr/bin/env python

import serial
import struct
import time
import threading
import queue
from loguru import logger
import sys # For logger setup in main
import json # for easy print
import math # calculate distance with angles
import datetime # For timestamping CSV logs
import os
try:
    from .simulation_kogger import simu_serial as simu
except Exception as e:
    print("Can't import simu:"+str(e)+", try other way")
try:
    from simulation_kogger import simu_serial as simu
except Exception as e:
    print("Can't import simu:"+str(e))

SAVE_CSV = True

class CsvLogger:
    def __init__(self, filename):
        self.filename = filename
        self.filepath = "log/" + filename
        self.file = None
        self.lock = threading.Lock()
        self._open_file()

    def _open_file(self):
        try:
            # Get the directory from the filepath
            log_dir = os.path.dirname(self.filepath)
            
            # Create the directory if it doesn't exist
            if log_dir: # Ensure log_dir is not an empty string
                os.makedirs(log_dir, exist_ok=True)
            
            self.file = open(self.filepath, 'a', buffering=1) # Line buffered
            logger.info(f"CSV logging enabled to {self.filepath}")
        except Exception as e:
            logger.error(f"Failed to open CSV log file {self.filepath}: {e}")
            self.file = None

    def log(self, direction, message_bytes):
        if self.file:
            with self.lock:
                timestamp = datetime.datetime.now().isoformat(sep=' ', timespec='microseconds')
                # Escape double quotes in the message bytes representation
                escaped_message = str(message_bytes).replace('"', '""')
                self.file.write(f'{timestamp},{direction},"' + escaped_message + '"\n')

    def __del__(self):
        if self.file and not self.file.closed:
            self.file.close()

def setup_logging(level="INFO", file=""):
    """
    Configures the module's logger.
    This function can be called from an external script to set the desired log level.
    :param level: The logging level to set (e.g.,"TRACE"<"DEBUG"<"INFO"<"SUCCESS"<"WARNING"<"ERROR"<"CRITICAL").
    :param file: The file path to save logs. If empty, logs are not saved to a file.
    If file is not defined, it doesn't create a new file, and use the old one
    If the level is the same as the current one, it does nothing
    """
    current_file=""
    current_level=-1
    for handler_id, handler in logger._core.handlers.items():
        # The '_name' attribute usually holds the string path for file sinks
        if isinstance(handler._name, str) and "/" in handler._name:
            current_file = handler._name
            current_level= handler._levelno
    if file != current_file or logger.level(level).no!=current_level:
        logger.remove()
        logger.add(sys.stdout, level=level, format="<green>{time:YYYY-MM-DD HH:mm:ss.SSS}</green> | <level>{level: <8}</level> | <cyan>{name}</cyan>:<cyan>{function}</cyan>:<cyan>{line}</cyan> - <level>{message}</level>")
        if file != "":
            logger.add(file, level=level, format="{time:YYYY-MM-DD HH:mm:ss.SSS} | {level: <8} | {name}:{function}:{line} - {message}")
            logger.info(f"Kogger Protocol Driver: Loguru logging configured to level {level} and file {file}.")
        else:
            logger.info(f"Kogger Protocol Driver: Loguru logging configured to level {level}.")

# Protocol Constants
SYNC1 = 0xBB
SYNC2 = 0x55
KEY_CONFIRM = 0xC96B5D4A

# Command IDs from PDF KS_SBP_100 Rev 3.0.7, Page 5
# Measurement data
ID_TIMESTAMP    = 0x01 # Timestamp
ID_DIST         = 0x02 # Distance data
ID_CHART        = 0x03 # Chart data in reflection patterns
ID_ATTITUDE     = 0x04 # Attitude
ID_TEMP         = 0x05 # Temperature data

# Settings data
ID_DATASET      = 0x10 # Dataset management for automatic output
ID_DIST_SETUP   = 0x11 # Detection Settings to Get Distance
ID_CHART_SETUP  = 0x12 # Chart Settings
ID_DSP          = 0x13 # DSP settings (No detailed spec in PDF beyond listing)
ID_TRANSC       = 0x14 # Transceiver settings
ID_SND_SPD      = 0x15 # Sound speed settings
ID_PIN          = 0x16 # Pin functions settings (No detailed spec in PDF beyond listing)
ID_BUS          = 0x17 # Bus settings (No detailed spec in PDF beyond listing)
ID_UART         = 0x18 # UART settings
ID_I2C          = 0x19 # I2C settings (No detailed spec in PDF beyond listing)
ID_CAN          = 0x1A # CAN settings (No detailed spec in PDF beyond listing)
ID_IMU_SETUP    = 0x1B # IMU settings (In developing)

# System
ID_VERSION      = 0x20 # Software and hardware version information (In developing)
ID_MARK         = 0x21 # Setting the mark of continuous work (non-reboot) device
ID_DIAG         = 0x22 # Diagnostic data (In developing)
ID_FLASH        = 0x23 # Work with built-in non-volatile memory
ID_BOOT         = 0x24 # Boot device
ID_UPDATE       = 0x25 # Firmware update

# Navigation & Signal
ID_NAV            = 0x64 # Navigation data (Latitude, Longitude, Accuracy)
ID_SIGNAL_ENCODER = 0xf6 #old 0x66 # Signal Encoder Data 
ID_SIGNAL_DECODER = 0xf7 #old 0x67 # Signal Decoder Data 
ID_USBL_SOLUTION  = 0x65 # USBL Solution Data
ID_MODEM_SOLUTION = 0x66 # Modem Solution Data
ID_USBL_CONTROL   = 0x68 # USBL Acoustic Control (Pinging and Auto-Response)
ID_MODEM_CONTROL  = 0x69 # USBL modem control
ID_DVL_VEL        = 0x79 # DVL Velocity data (0x79 decimal 121)



# MODE field - Type (Bits 0:1) 
TYPE_RESERVED = 0
TYPE_CONTENT  = 1
TYPE_SETTING  = 2
TYPE_GETTING  = 3

# MODE field - Response (Bit 7) 
RESPONSE_REQUEST_FLAG = 1

# RESP Codes (Response codes from device) 
RESP_NONE           = 0
RESP_OK             = 1
RESP_ERR_CHECKSUM   = 2
RESP_ERR_PAYLOAD    = 3
RESP_ERR_ID         = 4
RESP_ERR_VERSION    = 5
RESP_ERR_TYPE       = 6
RESP_ERR_KEY        = 7
RESP_ERR_RUNTIME    = 8

class KoggerSBPDevice:
    """
    Python driver for the Kogger Serial Binary Protocol (SBP).

    This class implements methods to interact with a Kogger device
    using the SBP, including sending commands, receiving responses,
    and handling unsolicited messages via a separate reader thread.

    Implemented based on "Serial Binary Protocol (SBP) specification",
    Document Number: KS_SBP_100, Revision: 3.0.7.

    Note on commands without detailed specs in PDF (ID_DSP, ID_PIN, ID_BUS, ID_I2C, ID_CAN):
    Constants for these IDs are defined, but corresponding methods are not implemented
    due to lack of payload structure information in the provided PDF.
    """
    def __init__(self, port, baudrate=921600, device_address=0x0, default_timeout=1.0, serial_read_timeout=0.1, timestamp=None, vehicleName=None, save_csv=SAVE_CSV, log_level="NULL", log_file=""):
        """
        Initializes the Kogger SBP device interface.

        :param port: Serial port (e.g., 'COM3' on Windows, '/dev/ttyUSB0' on Linux), or None if using a mock.
        :param baudrate: Baud rate for serial communication (default 921600).
        :param device_address: Device address (0-15, default 0x0). 
        :param default_timeout: Default timeout in seconds for waiting for solicited command responses.
        :param serial_read_timeout: Timeout in seconds for individual serial read operations in the reader thread.
        :param timestamp: timestamp used for filename log
        :param vehicleName: name put in filename log 
        :param save_csv: True:Save log file
        :param log_level: The logging level to set (e.g.,"TRACE"<"DEBUG"<"INFO"<"SUCCESS"<"WARNING"<"ERROR"<"CRITICAL" or "NULL" to disable).
        """
        self.port = port
        self.baudrate = baudrate
        self.device_address = device_address & 0x0F
        self.serial_conn = None
        self.default_timeout = default_timeout
        self.serial_read_timeout = serial_read_timeout

        self._reader_thread = None
        self._stop_event = threading.Event()
        self._response_handlers = {}
        self._response_lock = threading.Lock()

        self._callbacks = {}
        self._precallbacks = {}
        self._default_callback = None
        self._nmea_callback = None
        self._unsolicited_queue = queue.Queue()
        if timestamp == None:
            timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
        if vehicleName == None:
            vehicleName = "AUV"
        self._vehicleType = vehicleName[:3]
        self.usbl_data = dict()
        self._receive_buffer = bytearray()
        self._usbl_filter_echo = -1 # Set time.time() when ping sent
        self._usbl_filter_echo_enable = True # Set True = enable echo filter, False = disabled
        self._usbl_filter_echo_orig_received = False # Set False = first message not received, True=Message received
        # Data for synchronization slot
        self._sync_slot_total = 0
        self._sync_slot_index = 0
        self._sync_slot_duration = 0
        self._sync_enable_delay = False
        # Threading control variables
        self._send_lock = threading.Lock()
        self._is_sending = False
        self._send_last_response = True # First set to True, need the second to get the real value
        
        # Stores the latest data to be sent
        self._latest_address = 255
        self._latest_timeout = 0

        if log_level != "NULL":
            setup_logging(level=log_level, file=log_file)
        logger.info(f"KoggerSBPDevice configured for port {self.port}, baudrate {self.baudrate}, address {self.device_address}")
        self._csv_logger = None
        if save_csv:
            CSV_LOG_FILE = timestamp + "_" + vehicleName + "_usbl.csv"
            self._csv_logger = CsvLogger(CSV_LOG_FILE)

    def _calculate_fletcher16(self, data_bytes):
        """
        Calculates Fletcher-16 checksum for a byte array. 
        
        :param data_bytes: Bytes over which checksum is calculated (ROUTE, MODE, ID, LENGTH, PAYLOAD).
        :return: Tuple (CHECK1, CHECK2).
        """
        check1 = 0
        check2 = 0
        for byte_val in data_bytes:
            check1 = (check1 + byte_val) % 256
            check2 = (check2 + check1) % 256
        return check1, check2

    def _wait_until_modulo_slot_precise(
        self,
        slot_total: int,
        slot_index: int,
        slot_duration: float,
        enable_sleep: bool = True,
        spin_buffer: float = 0.002
    ):
        """
        Waits precisely until the specific slot index is reached.
        Args:
            slot_total (int): The total number of slots in one full cycle.
            slot_index (int): The specific slot to wait for (0 to slot_total-1).
            slot_duration (float): The max delay between 2 slots (duration of 1 slot).
            enable_sleep (bool): If False, uses 100% CPU busy-wait (extreme precision).
            spin_buffer (float): Time in seconds to switch from sleep to busy-wait.
        """
        if slot_duration == 0:
            return -1
        # 1. Calculate the full cycle parameters based on your inputs
        # The "modulo" is now the total time of all slots combined
        full_cycle_duration = slot_duration * slot_total
        # The "offset" is where your specific slot begins
        target_offset = slot_duration * slot_index
        now = time.time()

        # 2. Determine where we are in the current cycle
        current_pos = now % full_cycle_duration

        # 3. Calculate time remaining to reach the target offset
        # The logic handles wrap-around automatically
        wait_time = (target_offset - current_pos) % full_cycle_duration
        target_time = now + wait_time
        if enable_sleep == False:
            return target_time

        # 4. Hybrid Wait Strategy
        # Only sleep if the wait is significant enough (longer than buffer)
        if wait_time > spin_buffer:
            time.sleep(wait_time - spin_buffer)

        # 5. Precision Spin (Busy Wait)
        # This loop burns CPU for the final milliseconds to ensure accuracy
        while True:
            if time.time() >= target_time:
                break
        return target_time

    def connect(self):
        """
        Establishes the serial connection and starts the dedicated reader thread.
        
        :return: True if connection is successful, False otherwise.
        """
        if self.serial_conn and hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open: # Check for mock port compatibility
            logger.info("Already connected.")
            return True
        try:
            if self.port is None and not (hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open): # For mock port assignment
                logger.info("Serial port is None, connecting to simulation mock.")
                self.serial_conn = simu.Serial(self.port, self.baudrate, timeout=self.serial_read_timeout)
            if self.port is not None: # Only create serial.Serial if port is specified (not a mock)
                 self.serial_conn = serial.Serial(self.port, self.baudrate, timeout=self.serial_read_timeout)
            if not (hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open): # If still not open (e.g. mock not opened)
                logger.error("Connection object is not open after attempting to initialize.")
                return False
            self._stop_event.clear() 
            self._reader_thread = threading.Thread(target=self._reader_thread_loop, daemon=True)
            self._reader_thread.start()
            display_port = self.port if self.port is not None else getattr(self.serial_conn, 'port', 'mock port')
            logger.success(f"Successfully connected to {display_port} at {self.baudrate} and started reader thread.")
            return True
        except serial.SerialException as e:
            logger.error(f"Error connecting to {self.port}: {e}")
            self.serial_conn = None
            return False
        except AttributeError as e: 
            logger.error(f"AttributeError during connect: {e}. This might happen if 'port' is None and 'serial_conn' was not pre-assigned for testing.")
            return False
        except Exception as e: # Catch other potential errors
            logger.error(f"An unexpected error occurred during connect: {e}", exc_info=True)
            self.serial_conn = None
            return False

    def disconnect(self):
        """
        Stops the reader thread and closes the serial connection gracefully.
        """
        # Determine if we are trying to disconnect a non-existent or already closed connection
        was_connected = False
        if self.serial_conn:
            if hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open:
                was_connected = True
            elif not hasattr(self.serial_conn, 'is_open'): # Mock object might not have is_open initially
                 if self._reader_thread and self._reader_thread.is_alive(): # If thread is running, assume it was "connected"
                      was_connected = True
        if not was_connected and not (self._reader_thread and self._reader_thread.is_alive()):
            logger.warning("Not connected or already disconnected.")
            return
        logger.debug("Stopping reader thread...")
        self._stop_event.set() 
        if self._reader_thread and self._reader_thread.is_alive():
            self._reader_thread.join(timeout=self.default_timeout + 1) 
            if self._reader_thread.is_alive():
                logger.warning("Reader thread did not stop in time.")
        if self.serial_conn and hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open: 
            try:
                self.serial_conn.close()
                logger.info(f"Serial port {getattr(self.serial_conn, 'port', '')} closed.")
            except Exception as e:
                logger.error(f"Error closing serial connection: {e}")
        elif self.serial_conn: # Port existed but wasn't open
             logger.warning(f"Serial port {getattr(self.serial_conn, 'port', '')} was not open but cleaning up.")
        self.serial_conn = None 
        with self._response_lock:
            for handler_id, handler_info in list(self._response_handlers.items()): 
                if not handler_info['event'].is_set(): 
                    handler_info['response_data'] = {'error': 'disconnecting', 'id': handler_id}
                    handler_info['event'].set() 
            self._response_handlers.clear()
        logger.info("Disconnected and cleaned up.")

    def baudrate_update(self, new_baudrate):
        self.disconnect()
        self.baudrate = new_baudrate
        return self.connect()

    def _parse_frame_from_buffer(self):
        """
        Attempts to parse one complete SBP frame from the internal _receive_buffer.
        Handles finding SYNC bytes, validating length, and checksum. 
        :return: A dictionary representing the parsed frame if successful, None otherwise.
        """
        while True: 
            sync_pos = self._receive_buffer.find(bytes([SYNC1, SYNC2]))
            sync_sddbt = self._receive_buffer.find(bytes(list(b"$SDDBT")))
            # Add delay to release the reader thread
            time.sleep(0.0003)
            logger.debug(f"pos={sync_pos}, sdd={sync_sddbt}, size={len(self._receive_buffer)}")
            if sync_pos == -1 and sync_sddbt == -1: 
                if len(self._receive_buffer) > 1: 
                    self._receive_buffer = self._receive_buffer[-1:] if self._receive_buffer[-1] == SYNC1 else bytearray()
                return None 
            elif sync_sddbt != -1 and sync_pos == -1: # priority on sync_pos
                if sync_sddbt > 0: 
                    self._receive_buffer = self._receive_buffer[sync_sddbt:]
                if self._receive_buffer[-2]==0x0d and self._receive_buffer[-1]==0x0a:
                    logger.debug(f"SDDBT found!!!!{self._receive_buffer}")
                    ret = {'nmea':self._receive_buffer}
                    self._receive_buffer = bytearray()
                    return ret
                return None
            
            if sync_pos > 0: 
                logger.debug(f"Discarding {sync_pos} bytes before sync: {self._receive_buffer[:sync_pos].hex().upper()}")
                self._receive_buffer = self._receive_buffer[sync_pos:]

            if len(self._receive_buffer) < 6: 
                logger.debug(f"received_buffer={self._receive_buffer}<6")
                return None

            s1, s2, route_recv, mode_recv, id_recv, length_recv = struct.unpack_from('<BBBBBB', self._receive_buffer, 0)

            if s1 != SYNC1 or s2 != SYNC2:
                logger.debug(f"Invalid sync bytes {s1:#02x} {s2:#02x} after find (expected {SYNC1:#02x} {SYNC2:#02x}), removing first byte and retrying parse.")
                self._receive_buffer = self._receive_buffer[1:] 
                continue 

            expected_frame_len = 6 + length_recv + 2 
            if len(self._receive_buffer) < expected_frame_len: 
                logger.debug(f"receive_buffer ({self._receive_buffer}) < expected_frame_len ({expected_frame_len})")
                return None

            frame_bytes = self._receive_buffer[:expected_frame_len]
            payload_bytes = frame_bytes[6 : 6 + length_recv]
            check1_recv, check2_recv = struct.unpack_from('<BB', frame_bytes, 6 + length_recv)
            checksum_data_recv = frame_bytes[2 : 6 + length_recv] 
            calc_check1, calc_check2 = self._calculate_fletcher16(checksum_data_recv)

            checksum_ok = (calc_check1 == check1_recv and calc_check2 == check2_recv)
            if not checksum_ok:
                logger.error(f"Checksum error! Frame: {frame_bytes.hex().upper()}. Received C1={check1_recv:02X}, C2={check2_recv:02X}. Calculated C1={calc_check1:02X}, C2={calc_check2:02X}. Discarding frame.")
                self._receive_buffer = self._receive_buffer[expected_frame_len:] 
                continue 

            self._receive_buffer = self._receive_buffer[expected_frame_len:] 
            parsed_frame = {
                'route': route_recv, 'mode': mode_recv, 'id': id_recv, 'length': length_recv,
                'payload': payload_bytes, 'check1_recv': check1_recv, 'check2_recv': check2_recv,
                'checksum_ok': checksum_ok, 
                'raw_frame': frame_bytes
            }
            logger.debug(f"Parsed frame: ID={parsed_frame['id']:#02x}, Len={parsed_frame['length']}")

            is_device_resp_payload_flag = (parsed_frame['mode'] >> 7) & 0x01 
            received_type = parsed_frame['mode'] & 0x03 
            if is_device_resp_payload_flag and received_type == TYPE_CONTENT and parsed_frame['length'] == 3: # RESP message structure 
                resp_code, cmd_chk1, cmd_chk2 = struct.unpack('<BBB', parsed_frame['payload'])
                parsed_frame['resp_code'] = resp_code
                parsed_frame['original_cmd_chk1'] = cmd_chk1 
                parsed_frame['original_cmd_chk2'] = cmd_chk2
            
            return parsed_frame
        return None 

    

    def _parse_message(self, message_frame):
        """
        Routes a raw message frame to the correct payload parser based on its ID.
        This is used for unsolicited messages to avoid re-triggering command sends.
        :param message_frame: The raw message frame dictionary from _parse_frame_from_buffer.
        :return: A dictionary with the parsed data, or the original frame if no parser is found.
        """
        parsers = {
            ID_TIMESTAMP: self._parse_timestamp_payload,
            ID_DIST: self._parse_distance_payload,
            ID_CHART: self._parse_chart_data_payload,
            ID_ATTITUDE: self._parse_attitude_payload,
            ID_TEMP: self._parse_temperature_payload,
            ID_DATASET: self._parse_dataset_config_payload,
            ID_DIST_SETUP: self._parse_distance_setup_payload,
            ID_CHART_SETUP: self._parse_chart_setup_payload,
            ID_DSP: self._parse_dsp_payload,
            ID_TRANSC: self._parse_transceiver_settings_payload,
            ID_SND_SPD: self._parse_sound_speed_payload,
            ID_UART: self._parse_uart_config_payload,
            ID_VERSION: self._parse_version_info_payload,
            ID_MARK: self._parse_mark_status_payload,
            ID_DIAG: self._parse_diagnostics_payload,
            ID_NAV: self._parse_navigation_data_payload,
            ID_DVL_VEL: self._parse_dvl_velocity_data_payload,
            ID_SIGNAL_ENCODER: self._parse_signal_encoder_data_payload,
            ID_SIGNAL_DECODER: self._parse_signal_decoder_data_payload,
            ID_USBL_SOLUTION: self._parse_usbl_solution_payload,
            ID_MODEM_SOLUTION: self._parse_modem_solution_payload,
        }
        parser = parsers.get(message_frame['id'])
        if parser:
            # Call the appropriate parser with the frame
            return parser(message_frame)
        
        logger.debug(f"No specific parser for unsolicited message ID {message_frame['id']:#02x}. Returning raw frame.")
        return message_frame

    def _reader_thread_loop(self):
        """Target function for the reader thread."""
        logger.info("Reader thread started.")
        while not self._stop_event.is_set():
            try:
                # Check if serial_conn is valid and open before reading
                if not (self.serial_conn and hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open):
                    logger.debug("Reader thread: serial connection not available or not open. Pausing.")
                    time.sleep(0.1)
                    if self._stop_event.is_set(): break
                    continue # Re-check connection status in next iteration

                bytes_to_read = self.serial_conn.in_waiting or 1 
                data = self.serial_conn.read(bytes_to_read)
                if data:
                    self._receive_buffer.extend(data)
                    logger.debug(f"Reader thread received: {data.hex()}, buffer size: {len(self._receive_buffer)}")
                    logger.debug(f"FULL MESSAGE: {self._receive_buffer}")
                    if self._csv_logger: self._csv_logger.log("RECEIVED", data)

                while True: 
                    parsed_frame = self._parse_frame_from_buffer()
                    if parsed_frame is None:
                        break 
                    if 'nmea' in parsed_frame:
                        logger.debug("nmea frame received")
                        if self._nmea_callback:
                            try: self._nmea_callback(parsed_frame)
                            except Exception as e: logger.error(f"Error in nmea callback: {e}", exc_info=True)
                        break

                    logger.debug(f"Reader dispatching frame ID: {parsed_frame['id']:#02x}")
                    handled_as_solicited = False
                    with self._response_lock:
                        if parsed_frame['id'] in self._response_handlers:
                            handler = self._response_handlers[parsed_frame['id']]
                            if handler['response_data'] is None: 
                                handler['response_data'] = parsed_frame
                                handler['event'].set() 
                                handled_as_solicited = True
                                logger.debug(f"Reader: Dispatched solicited response for ID {parsed_frame['id']:#02x}")
                            else:
                                logger.warning(f"Reader: Response handler for ID {parsed_frame['id']:#02x} already had data. New frame ignored for this handler.")

                    if not handled_as_solicited:
                        logger.debug(f"Reader: Frame ID {parsed_frame['id']:#02x} is unsolicited.")
                        
                        # Parse the message payload without sending a new command
                        parsed_message = self._parse_message(parsed_frame)

                        if parsed_frame['id'] in self._precallbacks or parsed_frame['id'] in self._callbacks:
                            ret = 1 # return value of precallback, 1=send callback, 0=No
                            if parsed_frame['id'] in self._precallbacks:
                                try: 
                                    ret = self._precallbacks[parsed_frame['id']](parsed_message)
                                except Exception as e: logger.error(f"Error in precallback for ID {parsed_frame['id']:#02x}: {e}", exc_info=True)
                            if ret==1 and parsed_frame['id'] in self._callbacks:
                                try: 
                                    self._callbacks[parsed_frame['id']](parsed_message)
                                except Exception as e: logger.error(f"Error in callback for ID {parsed_frame['id']:#02x}: {e}", exc_info=True)
                        elif self._default_callback:
                            try: self._default_callback(parsed_message)
                            except Exception as e: logger.error(f"Error in default callback: {e}", exc_info=True)
                        else:
                            try: self._unsolicited_queue.put_nowait(parsed_message)
                            except queue.Full: logger.warning(f"Unsolicited message queue full. Discarding message ID {parsed_frame['id']:#02x}.")
            
            except serial.SerialException as se:
                logger.error(f"Serial error in reader thread: {se}. Stopping thread.")
                self._stop_event.set() 
                with self._response_lock:
                    for handler_id, handler_info in list(self._response_handlers.items()): 
                        if not handler_info['event'].is_set(): 
                            handler_info['response_data'] = {'error': 'serial_exception_in_reader', 'id': handler_id, 'details': str(se)}
                            handler_info['event'].set()
                break 
            except Exception as e:
                logger.error(f"Unexpected error in reader thread: {e}", exc_info=True) 
                time.sleep(0.01) 
        logger.info("Reader thread finished.")

    def _build_route_field(self, dev_addr=None):
        """Builds the ROUTE field (U1). """
        if dev_addr is None: dev_addr = self.device_address
        return dev_addr & 0x0F 

    def _build_mode_field(self, type_val, version_val=0, request_resp=False, mark_val=False):
        """Builds the MODE field (U1). """
        mode = (type_val & 0x03) 
        mode |= ((version_val & 0x07) << 3) 
        if mark_val: mode |= (1 << 6) 
        if request_resp: mode |= (1 << 7) 
        return mode

    def _build_frame(self, cmd_id, payload=b'', type_val=TYPE_GETTING, version_val=0, request_resp_flag=True, target_dev_addr=None):
        """Constructs a complete SBP frame ready for sending. """
        if len(payload) > 128: 
            raise ValueError("Payload length cannot exceed 128 bytes as per protocol. ")
        
        route = self._build_route_field(dev_addr=target_dev_addr)
        mode = self._build_mode_field(type_val, version_val, request_resp_flag)
        length = len(payload)

        checksum_data = bytes([route, mode, cmd_id, length]) + payload
        check1, check2 = self._calculate_fletcher16(checksum_data)

        frame = bytes([SYNC1, SYNC2, route, mode, cmd_id, length]) + payload + bytes([check1, check2])
        return frame

    def _send_frame(self, frame):
        """Sends a pre-built frame over the serial connection."""
        if not (self.serial_conn and hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open):
            logger.error("Serial connection is not open for sending.")
            return False
        try:
            self.serial_conn.write(frame)
            logger.debug(f"Sent: {frame.hex().upper()}")
            if self._csv_logger: self._csv_logger.log("SENT", frame)
            return True
        except serial.SerialException as e: # Catch pyserial specific errors
            logger.error(f"SerialException during send_frame: {e}")
            self._stop_event.set() 
            return False
        except Exception as e: # Catch other errors like if serial_conn is a mock without write
             logger.error(f"General Exception during send_frame: {e}")
             self._stop_event.set()
             return False


    def _execute_command(self, cmd_id, payload=b'', type_val=TYPE_GETTING, version_val=0, request_resp_flag=True, target_dev_addr=None, expect_content=True):
        """Core internal function to send a command and wait for its specific response."""
        if not (self.serial_conn and hasattr(self.serial_conn, 'is_open') and self.serial_conn.is_open): 
            logger.error("Not connected. Cannot execute command.")
            return None

        frame_to_send = self._build_frame(cmd_id, payload, type_val, version_val, request_resp_flag, target_dev_addr)
        
        event = threading.Event()
        handler_key = cmd_id 
        
        with self._response_lock:
            if handler_key in self._response_handlers:
                logger.warning(f"Overwriting existing response handler for command ID {cmd_id:#02x}.")
            self._response_handlers[handler_key] = {'event': event, 'response_data': None}

        if not self._send_frame(frame_to_send):
            with self._response_lock: 
                if handler_key in self._response_handlers: 
                     del self._response_handlers[handler_key]
            return None 

        response_data = None
        try:
            if not event.wait(timeout=self.default_timeout):
                logger.warning(f"Timeout waiting for response for command ID {cmd_id:#02x}")
                return None 
            
            with self._response_lock: 
                if handler_key in self._response_handlers:
                    response_data = self._response_handlers[handler_key]['response_data']
                else: 
                    logger.warning(f"Response handler for {cmd_id:#02x} missing after event was set.")
                    return None 

            if response_data is None: 
                 logger.error(f"Critical: Event set for command {cmd_id:#02x} but no response data found.")
                 return None

            if 'error' in response_data: 
                logger.error(f"Command {cmd_id:#02x} failed: {response_data['error']}{'; Details: ' + response_data['details'] if 'details' in response_data else ''}")
                return None 

            if 'resp_code' in response_data and response_data['id'] == cmd_id and response_data['resp_code'] != RESP_OK:
                logger.warning(f"Command {cmd_id:#02x} received error RESP: {self.get_resp_code_meaning(response_data['resp_code'])}")
                return response_data 

            if expect_content:
                if response_data['id'] == cmd_id and (response_data['mode'] & 0x03) == TYPE_CONTENT and 'resp_code' not in response_data:
                    return response_data 
                else:
                    logger.warning(f"Command {cmd_id:#02x} expected content but received: ID={response_data.get('id', 'N/A'):#02x}, Mode={response_data.get('mode', 'N/A'):#02x}, RespCode={response_data.get('resp_code', 'N/A')}")
                    return response_data 
            else: 
                 if 'resp_code' in response_data and response_data['id'] == cmd_id and response_data['resp_code'] == RESP_OK:
                     return response_data 
                 else: 
                     logger.warning(f"Command {cmd_id:#02x} expected RESP_OK, received: ID {response_data.get('id', 'N/A'):#02x} with resp_code {response_data.get('resp_code', 'N/A')}")
                     return response_data 
        finally:
            with self._response_lock: 
                if handler_key in self._response_handlers:
                    del self._response_handlers[handler_key]
        return response_data

    def get_resp_code_meaning(self, code):
        """Returns a human-readable string for a device RESP code. """
        return {
            RESP_NONE: "RESP_NONE", RESP_OK: "RESP_OK", RESP_ERR_CHECKSUM: "RESP_ERR_CHECKSUM",
            RESP_ERR_PAYLOAD: "RESP_ERR_PAYLOAD", RESP_ERR_ID: "RESP_ERR_ID",
            RESP_ERR_VERSION: "RESP_ERR_VERSION", RESP_ERR_TYPE: "RESP_ERR_TYPE",
            RESP_ERR_KEY: "RESP_ERR_KEY", RESP_ERR_RUNTIME: "RESP_ERR_RUNTIME"
        }.get(code, f"Unknown RESP code: {code}")

    def register_precallback(self, message_id, callback_function):
        """Registers a pre-callback function for a specific unsolicited message ID."""
        self._precallbacks[message_id] = callback_function
        logger.debug(f"pre-Callback registered for unsolicited message ID {message_id:#02x}")

    def register_callback(self, message_id, callback_function):
        """Registers a callback function for a specific unsolicited message ID."""
        self._callbacks[message_id] = callback_function
        logger.debug(f"Callback registered for unsolicited message ID {message_id:#02x}")

    def unregister_callback(self, message_id):
        """Unregisters a callback for a specific unsolicited message ID."""
        if message_id in self._callbacks: del self._callbacks[message_id]
        if message_id in self._precallbacks: del self._precallbacks[message_id]
        logger.debug(f"Callback unregistered for unsolicited message ID {message_id:#02x}")

    def register_default_callback(self, callback_function):
        """Registers a default callback for any unhandled unsolicited messages."""
        self._default_callback = callback_function
        logger.debug("Default callback registered for unsolicited messages.")
        
    def unregister_default_callback(self):
        """Unregisters the default callback."""
        self._default_callback = None
        logger.debug("Default callback unregistered.")

    def register_nmea_callback(self, callback_nmea):
        """Registers a nmea callback for any nmea messages."""
        self._nmea_callback = callback_nmea
        logger.debug("NMEA callback registered for NMEA messages.")

    def unregister_nmea_callback(self):
        """Unregisters the nmea callback."""
        self._nmea_callback = None
        logger.debug("NMEA callback unregistered.")

    def get_unsolicited_message(self, block=True, timeout=None):
        """Retrieves a message from the unsolicited message queue."""
        try: return self._unsolicited_queue.get(block=block, timeout=timeout)
        except queue.Empty: return None 

    # --- Pre-defined callback ---

    def callback_usbl_solution(self, message):
        # Skip message received for the time after a valid message
        __DELAY_ECHO_REMOVER = 1
        if self._vehicleType=="AUV":
            # Add filter to be sure to receive the same ID for successive message
            __NB_SAME_ID_SUCC = 3
            # Or get the ID if the last ok was before this delay in seconds
            __DELAY_RESET_SAME_ID = 9999999
        else:
            __NB_SAME_ID_SUCC = 0
            __DELAY_RESET_SAME_ID = 9999999
        if not hasattr(self.callback_usbl_solution.__func__, '_tick_last_ok'):
            # Local variable
            self.callback_usbl_solution.__func__._tick_last_ok = time.time()
            self.callback_usbl_solution.__func__._last_snr = 0
            self.callback_usbl_solution.__func__._id_prev = 0
            self.callback_usbl_solution.__func__._id_set = 255
            self.callback_usbl_solution.__func__._cnt_same_id = 0

        parsed = json.loads(str(message).replace("nan", "'nan'").replace("'",'"'))
        logger.debug("callback_usbl_solution:"+str(json.dumps(parsed, indent=2)))

        # Low SNR = skip
        _delay_previous_ok = time.time() - self.callback_usbl_solution.__func__._tick_last_ok
        self.callback_usbl_solution.__func__._tick_last = time.time()
        if message.get('id', 255)==255 or message.get("snr", 0.0)<self.callback_usbl_solution.__func__._last_snr-2-_delay_previous_ok*0.5:
            return 0
        # Skip message if too successive
        if "timestamp_pi" in self.usbl_data and time.time() - self.usbl_data["timestamp_pi"] < __DELAY_ECHO_REMOVER:
            return 0
        # Check echo, filter if ping arrives too late
        logger.debug(f"self._usbl_filter_echo_enable={self._usbl_filter_echo_enable}, echo_orig={self._usbl_filter_echo_orig_received}")
        if self._usbl_filter_echo_enable==True and self._usbl_filter_echo_orig_received==True:
            logger.debug(f"diff={time.time()-self._usbl_filter_echo}, {(self._sync_slot_duration*self._sync_slot_total)-0.2}")
            if (self._sync_slot_duration == 0 and (time.time()-self._usbl_filter_echo <= 0.7)) or \
                (time.time()-self._usbl_filter_echo <= (self._sync_slot_duration*self._sync_slot_total)-0.2):
                return 0
        self._usbl_filter_echo_orig_received = True

        # 255 or wrong previous ID, set the previous correct ID
        if self.callback_usbl_solution.__func__._id_prev!=255 and self.callback_usbl_solution.__func__._id_prev == message.get('id', 255):
            self.callback_usbl_solution.__func__._cnt_same_id += 1
        else:
            self.callback_usbl_solution.__func__._cnt_same_id = 0

        if self.callback_usbl_solution.__func__._cnt_same_id >= __NB_SAME_ID_SUCC-1 or _delay_previous_ok > __DELAY_RESET_SAME_ID:
            # Set ID if valid
            self.callback_usbl_solution.__func__._id_set = message.get('id', 255)

        self.callback_usbl_solution.__func__._id_prev = message.get('id', 255)
        self.usbl_data = message
        self.usbl_data["id"] = self.callback_usbl_solution.__func__._id_set
        self.usbl_data["timestamp_pi"] = time.time()
        self.usbl_data["slot_index"] = self.get_slot_index_from_time(self.usbl_data["timestamp_pi"])
        self.callback_usbl_solution.__func__._last_snr = message.get("snr", 0)
        self.callback_usbl_solution.__func__._tick_last_ok = time.time()
        return 1

    # --- Payload Parsers ---

    def _parse_timestamp_payload(self, frame):
        if frame['length'] == 4:
            timestamp = struct.unpack('<I', frame['payload'])[0]
            return {'timestamp_ms': timestamp}
        logger.warning(f"Invalid length for timestamp payload: {frame['length']}")
        return None

    def _parse_distance_payload(self, frame):
        payload = frame['payload']
        resp_version = (frame['mode'] >> 3) & 0x07
        if resp_version == 0 and frame['length'] == 4:
            distance = struct.unpack('<I', payload)[0]
            return {'distance_mm': distance, 'version': 0}
        elif resp_version == 1 and frame['length'] == 8:
            number, strong, distance, width = struct.unpack('<BBIH', payload)
            return {'number': number, 'strong': strong, 'distance_mm': distance, 'width_mm': width, 'version': 1}
        logger.warning(f"Invalid length/version for distance payload: L={frame['length']}, V={resp_version}")
        return None

    def _parse_chart_data_payload(self, frame):
        if frame['length'] >= 6:
            try:
                seq_offset, res_mm, abs_offset = struct.unpack_from('<HHH', frame['payload'], 0)
                chart_data = frame['payload'][6:]
                return {'seq_offset': seq_offset, 'sample_resolution_mm': res_mm, 'abs_offset_samples': abs_offset, 'chart_data': chart_data}
            except struct.error as e:
                logger.error(f"Error unpacking chart data: {e}")
        return None

    def _parse_attitude_payload(self, frame):
        payload = frame['payload']
        resp_version = (frame['mode'] >> 3) & 0x07
        if resp_version == 0 and frame['length'] == 6:
            yaw, pitch, roll = struct.unpack('<hhh', payload)
            return {'yaw_deg': yaw / 100.0, 'pitch_deg': pitch / 100.0, 'roll_deg': roll / 100.0, 'version': 0}
        elif resp_version == 1 and frame['length'] == 16:
            w0, w1, w2, w3 = struct.unpack('<ffff', payload)
            return {'q_w0': w0, 'q_w1': w1, 'q_w2': w2, 'q_w3': w3, 'version': 1}
        logger.warning(f"Invalid length/version for attitude payload: L={frame['length']}, V={resp_version}")
        return None

    def _parse_temperature_payload(self, frame):
        if frame['length'] == 2:
            temp = struct.unpack('<h', frame['payload'])[0]
            return {'temperature_celsius': temp / 100.0}
        return None

    def _parse_dataset_config_payload(self, frame):
        if frame['length'] == 9:
            channel_id, period, mask = struct.unpack('<BII', frame['payload'])
            return {'channel_id': channel_id, 'channel_period_ms': period, 'channel_mask': mask}
        return None

    def _parse_distance_setup_payload(self, frame):
        resp_version = (frame['mode'] >> 3) & 0x07
        if resp_version == 1 and frame['length'] == 8:
                start_offset, max_dist = struct.unpack('<II', frame['payload'])
                return {'start_offset_mm': start_offset, 'max_distance_mm': max_dist}
        elif resp_version == 2 and frame['length'] == 1:
                confidence_threshold_perc = struct.unpack('<B', frame['payload'])[0]
                return {'confidence_threshold_perc': confidence_threshold_perc}
        return None

    def _parse_chart_setup_payload(self, frame):
        if frame['length'] == 6:
            count, resolution, offset = struct.unpack('<HHH', frame['payload'])
            return {'sample_count': count, 'sample_resolution_mm': resolution, 'sample_offset_num': offset}
        return None

    def _parse_dsp_payload(self, frame):
        if frame['length'] == 1:
            horizontal_smoothing_factor = struct.unpack('<B', frame['payload'])[0]
            return {'horizontal_smoothing_factor': horizontal_smoothing_factor}
        return None

    def _parse_transceiver_settings_payload(self, frame):
        if frame['length'] == 4:
            freq, pulse, boost_val = struct.unpack('<HBB', frame['payload'])
            return {'frequency_khz': freq, 'pulse_count': pulse, 'boost_enabled': bool(boost_val)}
        return None

    def _parse_sound_speed_payload(self, frame):
        if frame['length'] == 4:
            speed = struct.unpack('<I', frame['payload'])[0]
            return {'sound_speed_mm_s': speed}
        return None

    def _parse_uart_config_payload(self, frame):
        resp_version = (frame['mode'] >> 3) & 0x07
        payload = frame['payload']
        if resp_version == 0 and frame['length'] == 9:
            key, uart_id, baud = struct.unpack('<IBI', payload)
            if key == KEY_CONFIRM: return {'uart_id': uart_id, 'baudrate': baud, 'version': 0}
        elif resp_version == 1 and frame['length'] == 6:
            key, uart_id, dev_addr = struct.unpack('<IBB', payload)
            if key == KEY_CONFIRM: return {'uart_id': uart_id, 'device_address': dev_addr, 'version': 1}
        elif resp_version == 2 and frame['length'] == 5:
            key, dev_def_address = struct.unpack('<IB', payload)
            if key == KEY_CONFIRM: return {'dev_def_address': dev_def_address}
        return None

    def _parse_version_info_payload(self, frame):
        resp_version = (frame['mode'] >> 3) & 0x07
        if resp_version==0 and frame['length'] == 34:
            try:
                data = struct.unpack('<ssHHHIssI16s', frame['payload'])
                return {'hw_ver_minor': int.from_bytes(data[0], "big"), 'hw_ver_major': int.from_bytes(data[1], "big"), 'hw_ver_ext': data[2],
                        'reserved1': data[3], 'reserved2': data[4], 'reserved3': data[5],
                        'boot_ver_minor': int.from_bytes(data[6], "big"), 'boot_ver_major': int.from_bytes(data[7], "big"),
                        'serial_number': data[8], 'part_nbr': data[9]
                        }
            except (struct.error, UnicodeDecodeError) as e:
                logger.error(f"Error parsing version info: {e}")
        elif resp_version==2 and frame['length'] == 9:
            try:
                data = struct.unpack('<sssssHss', frame['payload'])
                return {'run_mode': int.from_bytes(data[0], "big"), 'hw_ver_minor': int.from_bytes(data[1], "big"), 'hw_ver_major': int.from_bytes(data[2], "big"),
                        'boot_ver_minor': int.from_bytes(data[3], "big"), 'boot_ver_major': int.from_bytes(data[4], "big"), 'reserved1': data[5],
                        'fw_ver_minor': int.from_bytes(data[6], "big"), 'fw_ver_major': int.from_bytes(data[7], "big")
                        }
            except (struct.error, UnicodeDecodeError) as e:
                logger.error(f"Error parsing version all: {e}")
        return None

    def _parse_mark_status_payload(self, frame):
        if frame['length'] == 1:
            mark_status = struct.unpack('<B', frame['payload'])[0]
            return {'mark_active': bool(mark_status)}
        return None

    def _parse_diagnostics_payload(self, frame):
        if frame['length'] == 24:
            try:
                data = struct.unpack('<IhhhhHHHHH', frame['payload'])
                return {'uptime_ms': data[0], 'temp_imu_c': data[1] / 100.0, 'temp_cpu_c': data[2] / 100.0,
                        'temp_min_c': data[3] / 100.0, 'temp_max_c': data[4] / 100.0, 'sys_voltage_mv': data[5],
                        'boost_voltage_mv': data[6], 'detector_voltage_mv': data[7], 'detector_noise_mv': data[8],
                        'agc_gate_voltage_mv': data[9]}
            except struct.error as e:
                logger.error(f"Error parsing diagnostics: {e}")
        return None

    def _parse_navigation_data_payload(self, frame):
        if frame['length'] == 20:
            try:
                lat, lon, acc = struct.unpack('<ddf', frame['payload'])
                return {'latitude_deg': lat, 'longitude_deg': lon, 'accuracy_m': acc}
            except struct.error as e:
                logger.error(f"Error parsing nav data: {e}")
        return None

    def _parse_dvl_velocity_data_payload(self, frame):
        if frame['length'] == 68 and ((frame['mode'] >> 3) & 0x07) == 2:
            try:
                data = struct.unpack('<IIffffffffffcccccfff', frame['payload'])
                return {'flags': data[0], 'timestamp_ms': data[1], 'delta_time_s': data[2], 'latency_s': data[3],
                        'velocity_x_mps': data[4], 'velocity_y_mps': data[5], 'velocity_z_mps': data[6],
                        'velocity_z1_mps': data[7], 'velocity_z2_mps': data[8], 'uncertainty_x_mps': data[9],
                        'uncertainty_y_mps': data[10], 'uncertainty_z_mps': data[11], 'uncertainty_z1_mps': data[12],
                        'uncertainty_z2_mps': data[13], 'distance_z_m': data[14], 'distance_z1_m': data[15],
                        'distance_z2_m': data[16]}
            except struct.error as e:
                logger.error(f"Error parsing DVL data: {e}")
        return None

    def _parse_signal_encoder_data_payload(self, frame):
        if frame['length'] == 7:
            reserved1, bit_length, data = struct.unpack('<IHB', frame['payload'])
            return {'reserved1': reserved1, 'bit_length': bit_length, 'data': data}
        return None

    def _parse_signal_decoder_data_payload(self, frame):
        if frame['length'] == 47:
            try:
                data = struct.unpack('<IqqffffIIHB', frame['payload'])
                return {'timestamp_ms': data[0], 'carrier_us': data[1], 'carrier_count': data[2],
                        'source_level_db': data[3], 'source_snr_db': data[4], 'azimuth_deg': data[5],
                        'elevation_deg': data[6], 'reserved1': data[7], 'reserved2': data[8],
                        'bit_length': data[9], 'data': data[10]}
            except struct.error as e:
                logger.error(f"Error parsing signal decoder data: {e}")
        return None

    def _parse_usbl_solution_payload(self, frame):
        if frame['length'] == 152:
            try:
                data = struct.unpack('<BBHqIqfffffffffddffffddIff8f', frame['payload'])
                return {'id': data[0], 'role': data[1], 'reserved': data[2], 'timestamp_us': data[3],
                        'ping_counter': data[4], 'carrier_counter': data[5], 'distance_m': data[6],
                        'distance_unc': data[7], 'azimuth_deg': data[8], 'azimuth_unc': data[9],
                        'elevation_deg': data[10], 'elevation_unc': data[11], 'snr': data[12],
                        'beacon_x_m': data[13], 'beacon_y_m': data[14], 'beacon_latitude': data[15],
                        'beacon_longitude': data[16], 'beacon_depth': data[17], 'usbl_yaw': data[18],
                        'usbl_pitch': data[19], 'usbl_roll': data[20], 'usbl_latitude': data[21],
                        'usbl_longitude': data[22], 'last_iTOW': data[23], 'beacon_n_m': data[24],
                        'beacon_e_m': data[25], 'code_snr': list(data[26:])}
            except struct.error as e:
                logger.error(f"Error parsing USBL solution: {e}")
        elif frame['length'] == 52:
            # New v3.0 from Kogger when navigation fusion is disabled
            try:
                data = struct.unpack('<BBBBqIqfffffff', frame['payload'])
                return {'id': data[0], 'role': data[1], 'cmd_id': data[2], 'reserved': data[3], 
                        'timestamp_us': data[4], 'ping_counter': data[5], 'carrier_counter': data[6],
                        'distance_m': data[7], 'distance_unc': data[8],
                        'azimuth_deg': data[9], 'azimuth_unc': data[10], 'elevation_deg': data[11], 'elevation_unc': data[12],
                        'snr': data[13]}
            except struct.error as e:
                logger.error(f"Error parsing USBL solution: {e}")
        return None

    def _parse_modem_solution_payload(self, frame):
        if frame['length'] == 38:
            try:
                data = struct.unpack('<qqqQBBBBH', frame['payload'])
                return {'timestamp_us': data[0], 'carrier_us': data[1], 'carrier_counter': data[2], 'reserved1': data[3], 
                        'event_on_request_response': data[4], 'address_from': data[5], 'address_to': data[6],
                        'slot_id': data[7], 'payload_bit_length': data[8]}
            except struct.error as e:
                logger.error(f"Error parsing modem solution: {e}")
        return None

    def _parse_usbl_control_get_timeout_payload(self, frame):
        if frame['length'] == 4:
            timeout = struct.unpack('<I', frame['payload'])[0]
            return {'auto_response_timeout_us': timeout}
        logger.warning(f"Invalid length for USBL control get timeout payload: {frame['length']}")
        return None

    def _parse_usbl_control_get_filter_payload(self, frame):
        if frame['length'] == 1:
            address = struct.unpack('<B', frame['payload'])[0]
            return {'auto_response_filter_address': address}
        logger.warning(f"Invalid length for USBL control get filter payload: {frame['length']}")
        return None

    def _parse_usbl_control_get_payload_payload(self, frame):
        if frame['length'] == 1:
            address = struct.unpack('<B', frame['payload'])[0]
            return {'auto_response_payload_address': address}
        logger.warning(f"Invalid length for USBL control get payload payload: {frame['length']}")
        return None

    # --- Command Implementations ---

    def get_timestamp(self):
        response = self._execute_command(ID_TIMESTAMP, request_resp_flag=False)
        return self._parse_timestamp_payload(response) if response else None

    def get_distance(self, version=0):
        response = self._execute_command(ID_DIST, version_val=version, request_resp_flag=False)
        if response:
            resp_version = (response['mode'] >> 3) & 0x07
            if resp_version != version:
                logger.warning(f"Requested distance v{version}, received v{resp_version}")
            return self._parse_distance_payload(response)
        return None

    # TODO : This takes ~160ms to respond (heavy message) in several messages
    # so it's not working…
    def get_chart_data(self):
        response = self._execute_command(ID_CHART, request_resp_flag=False)
        return self._parse_chart_data_payload(response) if response else None

    def get_attitude(self, version=0):
        response = self._execute_command(ID_ATTITUDE, version_val=version, request_resp_flag=False)
        if response:
            resp_version = (response['mode'] >> 3) & 0x07
            if resp_version != version:
                logger.warning(f"Requested attitude v{version}, received v{resp_version}")
            return self._parse_attitude_payload(response)
        return None

    def get_temperature(self):
        response = self._execute_command(ID_TEMP, request_resp_flag=False)
        return self._parse_temperature_payload(response) if response else None

    def set_dataset_config(self, channel_id, channel_period_ms, channel_mask):
        if not (0 <= channel_id <= 2):
            raise ValueError("Channel ID must be between 0 and 2.")
        payload = struct.pack('<BII', channel_id, channel_period_ms, channel_mask)
        response = self._execute_command(ID_DATASET, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_dataset_config(self, channel_id_to_request=0):
        payload = struct.pack('<B', channel_id_to_request)
        response = self._execute_command(ID_DATASET, payload, TYPE_GETTING, version_val=0, request_resp_flag=False)
        return self._parse_dataset_config_payload(response) if response else None
    
    def set_distance_setup(self, start_offset_mm, max_distance_mm):
        payload = struct.pack('<II', start_offset_mm, max_distance_mm)
        response = self._execute_command(ID_DIST_SETUP, payload, TYPE_SETTING, version_val=1, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_distance_setup(self):
        response = self._execute_command(ID_DIST_SETUP, version_val=1, request_resp_flag=False)
        return self._parse_distance_setup_payload(response) if response else None

    def set_confidence_threshold(self, confidence_threshold_perc):
        payload = struct.pack('<B', confidence_threshold_perc)
        response = self._execute_command(ID_DIST_SETUP, payload, TYPE_SETTING, version_val=2, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_confidence_threshold(self):
        response = self._execute_command(ID_DIST_SETUP, version_val=2, request_resp_flag=False)
        return self._parse_distance_setup_payload(response) if response else None

    def set_chart_setup(self, sample_count, sample_resolution_mm, sample_offset_num):
        payload = struct.pack('<HHH', sample_count, sample_resolution_mm, sample_offset_num)
        response = self._execute_command(ID_CHART_SETUP, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_chart_setup(self):
        response = self._execute_command(ID_CHART_SETUP, request_resp_flag=False)
        return self._parse_chart_setup_payload(response) if response else None

    def set_horizontal_smoothing_factor(self, horizontal_smoothing_factor):
        payload = struct.pack('<B', horizontal_smoothing_factor)
        response = self._execute_command(ID_DSP, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_horizontal_smoothing_factor(self):
        response = self._execute_command(ID_DSP, request_resp_flag=False)
        return self._parse_dsp_payload(response) if response else None

    def set_transceiver_settings(self, frequency_khz, pulse_count, boost_enabled):
        payload = struct.pack('<HBB', frequency_khz, pulse_count, 1 if boost_enabled else 0)
        response = self._execute_command(ID_TRANSC, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_transceiver_settings(self):
        response = self._execute_command(ID_TRANSC, request_resp_flag=False)
        return self._parse_transceiver_settings_payload(response) if response else None

    def set_sound_speed(self, sound_speed_mm_s):
        payload = struct.pack('<I', sound_speed_mm_s)
        response = self._execute_command(ID_SND_SPD, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_sound_speed(self):
        response = self._execute_command(ID_SND_SPD, request_resp_flag=False)
        return self._parse_sound_speed_payload(response) if response else None

    def set_uart_config(self, uart_id, baudrate=None, new_dev_address=None):
        if new_dev_address is not None and baudrate is None:
            if not (0 <= new_dev_address <= 15): raise ValueError("Device address must be 0-15.")
            payload = struct.pack('<IBB', KEY_CONFIRM, uart_id, new_dev_address)
            version = 1
        elif baudrate is not None and new_dev_address is None:
            payload = struct.pack('<IBI', KEY_CONFIRM, uart_id, baudrate)
            version = 0
        else:
            raise ValueError("Only baudrate or new_dev_address must be provided, not both.")
        response = self._execute_command(ID_UART, payload, TYPE_SETTING, version, expect_content=False)
        if response and response.get('resp_code') == RESP_OK:
            if new_dev_address is not None: self.device_address = new_dev_address
            return True
        return False

    def get_uart_config(self, uart_id, version=0):
        if version not in [0, 1]: raise ValueError("Version must be 0 or 1.")
        payload = struct.pack('<IB', KEY_CONFIRM, uart_id)
        response = self._execute_command(ID_UART, payload, TYPE_GETTING, version, request_resp_flag=False)
        return self._parse_uart_config_payload(response) if response else None

    def set_uart_dev_def_address(self, dev_def_address):
        if not (0 <= dev_def_address <=14): raise ValueError("dev_def_address must be 0-14.")
        payload = struct.pack('<IB', KEY_CONFIRM, dev_def_address)
        response = self._execute_command(ID_UART, payload, TYPE_SETTING, version_val=2, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # !!! Not working !!!
    def get_uart_dev_def_address(self):
        #return None
        payload = struct.pack('<I', KEY_CONFIRM)
        response = self._execute_command(ID_UART, payload, TYPE_GETTING, version_val=2, request_resp_flag=False)
        return self._parse_uart_config_payload(response) if response else None

    def set_imu_calibration(self, calibrate_gyro=False, calibrate_accelerometer=False):
        logger.warning("IMU_SETUP is marked 'In developing' in protocol PDF.")
        results = {}
        payload = struct.pack('<I', KEY_CONFIRM)
        if calibrate_gyro:
            resp_g = self._execute_command(ID_IMU_SETUP, payload, TYPE_SETTING, 0, expect_content=False)
            results['gyro_cal_ok'] = resp_g and resp_g.get('resp_code') == RESP_OK
        if calibrate_accelerometer:
            resp_a = self._execute_command(ID_IMU_SETUP, payload, TYPE_SETTING, 1, expect_content=False)
            results['accel_cal_ok'] = resp_a and resp_a.get('resp_code') == RESP_OK
        return results

    def get_version_info(self):
        response = self._execute_command(ID_VERSION, request_resp_flag=False)
        return self._parse_version_info_payload(response) if response else None

    def get_version_all(self):
        response = self._execute_command(ID_VERSION, version_val=2, request_resp_flag=False)
        return self._parse_version_info_payload(response) if response else None

    def set_mark(self):
        payload = struct.pack('<I', KEY_CONFIRM)
        response = self._execute_command(ID_MARK, payload, TYPE_SETTING, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_mark_status(self):
        response = self._execute_command(ID_MARK, request_resp_flag=False)
        return self._parse_mark_status_payload(response) if response else None

    def get_diagnostics(self):
        logger.warning("ID_DIAG is marked 'In developing' in protocol PDF.")
        response = self._execute_command(ID_DIAG, request_resp_flag=False)
        return self._parse_diagnostics_payload(response) if response else None

    def flash_operation(self, operation_type):
        if operation_type not in [0, 1, 2]: raise ValueError("Operation must be 0, 1, or 2.")
        payload = struct.pack('<I', KEY_CONFIRM)
        response = self._execute_command(ID_FLASH, payload, TYPE_SETTING, operation_type, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def save_settings_to_flash(self): return self.flash_operation(0)
    def restore_settings_from_flash(self): return self.flash_operation(1)
    def erase_flash_settings(self): return self.flash_operation(2)

    def boot_operation(self, operation_type):
        if operation_type not in [0, 1]: raise ValueError("Operation must be 0 or 1.")
        payload = struct.pack('<I', KEY_CONFIRM)
        if operation_type == 0: # Reboot
            frame = self._build_frame(ID_BOOT, payload, TYPE_SETTING, 0)
            if self._send_frame(frame):
                logger.info("Reboot command sent. Disconnecting.")
                time.sleep(0.2)
                self.disconnect()
                return True
            return False
        else: # Run FW
            response = self._execute_command(ID_BOOT, payload, TYPE_SETTING, 1, expect_content=False)
            return response and response.get('resp_code') == RESP_OK

    def reboot_device(self): return self.boot_operation(0)
    def run_firmware_from_bootloader(self): return self.boot_operation(1)

    def upload_firmware_update_chunk(self, packet_number, chunk):
        if len(chunk) > 126: raise ValueError("Chunk size exceeds 126 bytes.")
        if packet_number < 1: raise ValueError("Packet number must be >= 1.")
        payload = struct.pack('<H', packet_number) + chunk
        response = self._execute_command(ID_UPDATE, payload, TYPE_SETTING, 0, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_navigation_data(self):
        response = self._execute_command(ID_NAV, request_resp_flag=False)
        return self._parse_navigation_data_payload(response) if response else None

    def get_dvl_velocity_data(self):
        response = self._execute_command(ID_DVL_VEL, version_val=2, request_resp_flag=False)
        return self._parse_dvl_velocity_data_payload(response) if response else None

    def set_signal_encoder_data(self, bit_length, data_byte, reserved1=0):
        payload = struct.pack('<IHB', reserved1, bit_length, data_byte)
        response = self._execute_command(ID_SIGNAL_ENCODER, payload, TYPE_SETTING, 0, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def get_signal_encoder_data(self):
        response = self._execute_command(ID_SIGNAL_ENCODER, request_resp_flag=False)
        return self._parse_signal_encoder_data_payload(response) if response else None

    def get_signal_decoder_data(self):
        response = self._execute_command(ID_SIGNAL_DECODER, request_resp_flag=False)
        return self._parse_signal_decoder_data_payload(response) if response else None

    def get_usbl_solution(self):
        response = self._execute_command(ID_USBL_SOLUTION, request_resp_flag=False)
        return self._parse_usbl_solution_payload(response) if response else None

    def get_modem_solution(self):
        response = self._execute_command(ID_MODEM_SOLUTION, request_resp_flag=False)
        return self._parse_modem_solution_payload(response) if response else None

    def send_acoustic_ping(self, address=255, timeout_us=0):
        """
        Non-blocking call. Updates the address to be sent.
        If a wait is in progress, it updates the payload and returns immediately.
        If idle, it starts the background wait-and-send process.
        """
        with self._send_lock:
            # 1. ALWAYS update the data to the latest request
            self._latest_address = address
            self._latest_timeout = timeout_us
            # 2. If we are already waiting/sending, exit immediately
            if self._is_sending:
                return self._send_last_response 
            # 3. If idle, mark as busy and start the background thread
            self._is_sending = True
            # Create a daemon thread (won't prevent app from exiting)
            t = threading.Thread(target=self._threaded_send_worker)
            t.daemon = True 
            t.start()
            # Return True to indicate "New send sequence started"
            return self._send_last_response 

    def _threaded_send_worker(self):
        """
        Background worker that waits for the slot, THEN grabs the latest data,
        and executes the command.
        """
        try:
            # A. Perform the precise wait (This blocks this thread only)
            if self._sync_enable_delay == True:
                self._wait_until_modulo_slot_precise(
                    slot_total=self._sync_slot_total,
                    slot_index=self._sync_slot_index,
                    slot_duration=self._sync_slot_duration
                )
            # B. JUST BEFORE SENDING: Grab the latest address
            # This ensures we send the "last address received" during the wait
            with self._send_lock:
                address = self._latest_address
                timeout = self._latest_timeout
            # C. Execute Command
            payload = struct.pack('<IB', timeout, address)
            # Assuming _execute_command exists in your class
            response = self._execute_command(
                ID_USBL_CONTROL, 
                payload, 
                TYPE_SETTING, 
                1, 
                expect_content=False
            )
            self._usbl_filter_echo = time.time()
            self._usbl_filter_echo_orig_received = False
            self._send_last_response = (response and response.get('resp_code') == RESP_OK)
            # Optional: Log response here since we can't return it to the main thread
            # if response and response.get('resp_code') == RESP_OK:
            #     print("Ping sent successfully")
        except Exception as e:
            print(f"Error in acoustic sender: {e}")
        finally:
            # D. Release the flag so new calls can trigger a new cycle
            with self._send_lock:
                self._is_sending = False

    def get_slot_index_from_time(self, timestamp: float) -> int:
        """
        Calculates which slot index is active at a specific timestamp.
        Args:
            timestamp (float): The time to check (e.g., time.time()).
        Returns:
            int: The index of the slot (0 to total_slots - 1).
        """
        # 1. Calculate the full cycle duration
        full_cycle_duration = self._sync_slot_duration * self._sync_slot_total
        # 2. Find the position of the timestamp within the cycle (0 to cycle_duration)
        position_in_cycle = (timestamp+0.2*self._sync_slot_duration) % full_cycle_duration
        # 3. Determine which slot this position falls into
        # We use int() to floor the result, as slots are sequential "buckets"
        slot_index = int(position_in_cycle / self._sync_slot_duration)
        return slot_index

    def set_auto_response_timeout(self, timeout_us):
        payload = struct.pack('<I', timeout_us)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 3, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # 1-8 are addresses, 0: promisc address, 0xFFFFFFFF: disabled address slot
    def set_auto_response_filter(self, address):
        payload = struct.pack('<I', address)
        print("payload="+str(payload))
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 4, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    def set_auto_response_payload(self, address):
        if (not (0 <= address <= 8)) and address != 255:
            logger.warning(f"Auto response payload address should typically be 0-8. Got {address}.")
        payload = struct.pack('<B', address)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 5, expect_content=False)
        return response and response.get('resp_code') == RESP_OK


    ### NEW VERSION ###

    # Send request now or enable request trigger window.
    # In slot rotation, this is the key command used by active initiator AUV.
    # address : request address 0 to 7
    # cmd_id : logical command slot 0 to 7
    # reply_dist : response wait window based on max range
    # function : 0: default, 1:bitArray, 2:LLGeoAzimuth
    # payload_len : bit of modem payload
    def set_usbl_ping_request_direct(self, address, cmd_id, reply_dist=200000, function=0, payload_len=0):
        if (not (0 <= address <= 7)):
            logger.warning(f"Auto response payload address should typically be 0-7. Got {address}.")
        if (not (0 <= cmd_id <= 7)):
            logger.warning(f"Auto response payload cmd_id should typically be 0-7. Got {cmd_id}.")
        payload = struct.pack('<IBBIBH', 0, address, cmd_id, reply_dist, function, payload_len)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 1, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # Enable/disable transponder response window.
    # False:silent, True:always transponder
    def set_usbl_transponder(self, enable):
        timeout_us = 0
        if enable == False:
            timeout_us = 0
        else:
            timeout_us = 0xFFFFFFFF
        payload = struct.pack('<I', timeout_us)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 3, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # List for incoming request addresses
    # 0 to 7 and 0xff=unused
    # Should be an array of 8 bytes : [0,1,2,3,0xff,5,6,7]
    def set_usbl_request_address_filter(self, addresses):
        if len(addresses)!=8:
            logger.error(f"usbl request address filter should be an array of 8 bytes:{address}")
            return RESP_ERR_PAYLOAD
        for address in addresses:
            if address > 7 and address!=0xff:
                logger.error(f"usbl request address filter should be between [0;7] or 0xff:{address}")
                return RESP_ERR_PAYLOAD
        payload = struct.pack('<8B', addresses)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 4, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # Commands for each request/response events and modem payload handling
    # Use cmd_id one by one
    # cmd_id : slot index 0 to 7
    # event_on_receive_req_resp : True for request, False for response
    # cmd_id_replacement : New cmd_id to put on reception, -1 to disable replacement
    # address_replacement : New address_replacement to put on reception, -1 to disable replacement
    # send_back_ev_swap : True:Swap event direction, False:keep the same
    # receiver_function : 0:Default, 1:BitArray, 2:LLGeoAzimuth
    # receive_bit_length : Number of bits to wait during reception
    # sender_function : 0:Default, 1:BitArray, 2:LLGeoAzimuth
    # send_bit_length : Number of bits to send
    def set_usbl_cmd_config(self, cmd_id, event_on_receive_req_resp=False, 
                            cmd_id_replacement=-1, 
                            address_replacement=-1, 
                            send_back_ev_swap=False, 
                            receiver_function=0, receive_bit_length=0,
                            sender_function=0, send_bit_length=0):
        if (not (0 <= cmd_id <= 7)):
            logger.warning(f"USBL cmd config cmd_id should typically be 0-7. Got {cmd_id}.")
        payload = struct.pack('<IBBIBH', 
                              cmd_id,
                              event_on_request_response==1,
                              function, payload_len)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 6, expect_content=False)
        return response and response.get('resp_code') == RESP_OK

    # Runtime receive/suppress tuning.
    # enable = True enable monitor, False:disable
    def set_usbl_monitor_config(self, enable, echo_filter_response_us=400000, echo_filter_request_us=400000):
        payload = struct.pack('<II?', echo_filter_response_us, echo_filter_request_us, enable)
        response = self._execute_command(ID_USBL_CONTROL, payload, TYPE_SETTING, 7, expect_content=False)
        return response and response.get('resp_code') == RESP_OK


    # Set the sync mode, external to USBL
    def set_sync_mode(self, slot_total, slot_index, slot_duration, enable_delay):
        # enable_delay : use to wait slot when send_acoustic. Only for master!
        self._sync_slot_total = slot_total
        self._sync_slot_index = slot_index
        self._sync_slot_duration = slot_duration
        self._sync_enable_delay = enable_delay
        return True

    def set_echo_filter(self, enable):
        if enable == True:
            self._usbl_filter_echo_enable = True
        else:
            self._usbl_filter_echo_enable = False
        return True

    def get_auto_response_timeout(self):
        response = self._execute_command(ID_USBL_CONTROL, type_val=TYPE_GETTING, version_val=3, request_resp_flag=False)
        logger.warning("Toto="+str(response))
        return self._parse_usbl_control_get_timeout_payload(response) if response else None

    def get_auto_response_filter(self):
        response = self._execute_command(ID_USBL_CONTROL, type_val=TYPE_GETTING, version_val=4, request_resp_flag=False)
        return self._parse_usbl_control_get_filter_payload(response) if response else None

    def get_auto_response_payload(self):
        response = self._execute_command(ID_USBL_CONTROL, type_val=TYPE_GETTING, version_val=5, request_resp_flag=False)
        return self._parse_usbl_control_get_payload_payload(response) if response else None

    # --- Get variables ---

    def get_usbl_data(self):
        return self.usbl_data

    def get_usbl_distance_calc(self, depth=0):
        if depth==0:
            logger.warning("depth="+str(depth)+", can't calculate distance")
            return -1
        if "elevation_deg" not in self.usbl_data:
            logger.warning("No usbl receive, can't calculate distance")
            return -1
        else:
            # Formula is depth/cos((90-elevation_deg)*pi/380)
            return depth/math.cos((90-30)*3.14159265/180)

if __name__ == '__main__':
    # This block is for basic testing or direct execution of this file.
    # For more comprehensive testing, use a separate test script.
    # Setup logging to DEBUG level when running file directly
    setup_logging(level="DEBUG")

    logger.info("Kogger SBP Driver module loaded. This can be imported by other scripts.")
    logger.info(f"Key Confirm Constant: {KEY_CONFIRM:#0X}")
    # Example: Test checksum calculation
    # dev = KoggerSBPDevice(port=None) # For internal method access
    # chk_data = bytes([0x00, 0x01, 0x01, 0x04, 0x39, 0x30, 0x00, 0x00])
    # c1, c2 = dev._calculate_fletcher16(chk_data)
    # logger.debug(f"Checksum for {chk_data.hex()}: C1={c1:#02x}, C2={c2:#02x} (Expected 0x49, 0x95 for frame1 example)")