#! /usr/bin/env python
# Test all commands from kogger driver

import sys
import time
import struct # For unpacking in callbacks, if needed
from loguru import logger

# Ensure the kogger_protocol_driver.py is in the Python path
try:
    from kogger_protocol_driver import (
        KoggerSBPDevice,
        ID_TIMESTAMP,
        # Add other specific IDs if you want specific callbacks for them
    )
except ImportError:
    logger.critical("Failed to import KoggerSBPDevice. Make sure kogger_protocol_driver.py is in the same directory or Python path.")
    sys.exit(1)

# --- Loguru Setup for this script ---
logger.remove() # Remove default handler
LOG_LEVEL = "INFO" # Change to "DEBUG" for more detailed driver logs
logger.add(sys.stderr, level=LOG_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>")

# --- Configuration for Real Device ---
SERIAL_PORT = "/dev/ttyUSB0"
BAUDRATE = 115200
DEVICE_ADDRESS = 0
REQUEST_INTERVAL_SECONDS = 10  # How often to run the full cycle of get requests
COMMAND_PAUSE_SECONDS = 0.01   # Small pause between individual commands

# --- Callback for Unsolicited Messages ---
def handle_unsolicited_message(frame):
    """Default callback for unsolicited messages."""
    #logger.info(f"[UNSOLICITED MSG] ID: {frame['id']:#02x}, Mode: {frame['mode']:#02x}, Len: {frame['length']}, Payload: {frame['payload'].hex().upper()}")
    if frame['id'] == ID_TIMESTAMP and frame.get('checksum_ok') and \
       (frame['mode'] & 0x03) == 1 and frame['length'] == 4: # TYPE_CONTENT = 1
        try:
            timestamp = struct.unpack('<I', frame['payload'])[0]
            logger.info(f"  -> Unsolicited Timestamp: {timestamp} ms")
        except struct.error as e:
            logger.warning(f"  -> Failed to unpack unsolicited timestamp: {e}")
    # Add more specific parsing here if you expect other unsolicited messages frequently

def UpdateCounter(data, string, str_valid, str_timeout):
    if data: 
        logger.success("  Success to get "+str(string)+":"+str(data))
        str_valid.append(string) 
    else: 
        logger.warning("  Failed to get "+str(string))
        str_timeout.append(string)


def main():
    logger.info(f"Attempting to connect to Kogger device on {SERIAL_PORT} at {BAUDRATE} baud.")

    driver = KoggerSBPDevice(
        port=SERIAL_PORT,
        baudrate=BAUDRATE,
        device_address=DEVICE_ADDRESS,
        default_timeout=0.1 # Wait up to 2s for a solicited response
    )

    if not driver.connect():
        logger.error(f"Failed to connect to the device. Please check connections and permissions for {SERIAL_PORT}.")
        return

    driver.register_default_callback(handle_unsolicited_message)
    logger.info("Registered default callback for unsolicited messages.")

    try:
        logger.info(f"Successfully connected. Starting data polling cycle every {REQUEST_INTERVAL_SECONDS} seconds. Press Ctrl+C to stop.")
        loop_count = 0
        while True:
            str_valid = []
            str_timeout = []
            loop_count += 1
            logger.info(f"\n========= Request Cycle {loop_count} =========")

            # --- Measurement Data ---
            logger.info("--- Requesting Timestamp (get_timestamp) ---")
            data = driver.get_timestamp()
            UpdateCounter(data, "get_timestamp", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Distance v0 (get_distance) ---")
            data = driver.get_distance(version=0)
            UpdateCounter(data, "get_distance(version=0)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Distance v1 (get_distance) ---")
            data = driver.get_distance(version=1)
            UpdateCounter(data, "get_distance(version=1)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Chart Data (get_chart_data) ---")
            data = driver.get_chart_data()
            UpdateCounter(data, "get_chart_data()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Attitude v0 - Euler (get_attitude) ---")
            data = driver.get_attitude(version=0)
            UpdateCounter(data, "get_attitude(version=0)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Attitude v1 - Quaternion (get_attitude) ---")
            data = driver.get_attitude(version=1)
            UpdateCounter(data, "get_attitude(version=1)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Temperature (get_temperature) ---")
            data = driver.get_temperature()
            UpdateCounter(data, "get_temperature()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            # --- Settings Data (GET methods) ---
            UART_ID_TO_QUERY = 1 # Example UART ID from PDF
            CHANNEL_ID_TO_QUERY = 0 # Example: 0 for "all active" or default dataset

            logger.info(f"--- Requesting Dataset Config (Channel {CHANNEL_ID_TO_QUERY}) (get_dataset_config) ---")
            data = driver.get_dataset_config(channel_id_to_request=CHANNEL_ID_TO_QUERY)
            UpdateCounter(data, "get_dataset_config(channel_id_to_request=CHANNEL_ID_TO_QUERY)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Distance Setup (get_distance_setup) ---")
            data = driver.get_distance_setup()
            UpdateCounter(data, "get_distance_setup()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Chart Setup (get_chart_setup) ---")
            data = driver.get_chart_setup()
            UpdateCounter(data, "get_chart_setup()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Transceiver Settings (get_transceiver_settings) ---")
            data = driver.get_transceiver_settings()
            UpdateCounter(data, "get_transceiver_settings()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            data = driver.get_sound_speed()
            UpdateCounter(data, "get_sound_speed()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info(f"--- Requesting UART Config v0 (Baudrate, UART {UART_ID_TO_QUERY}) (get_uart_config) ---")
            data = driver.get_uart_config(uart_id=UART_ID_TO_QUERY, version=0)
            UpdateCounter(data, "get_uart_config(uart_id=1, version=0)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info(f"--- Requesting UART Config v1 (Dev Address, UART {UART_ID_TO_QUERY}) (get_uart_config) ---")
            data = driver.get_uart_config(uart_id=UART_ID_TO_QUERY, version=1)
            UpdateCounter(data, "get_uart_config(uart_id=1, version=1)", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            # --- System Data (GET methods) ---
            logger.info("--- Requesting Version Info (get_version_info) ---")
            data = driver.get_version_info()
            UpdateCounter(data, "get_version_info()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Mark Status (get_mark_status) ---")
            data = driver.get_mark_status()
            UpdateCounter(data, "get_mark_status()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting Diagnostics (get_diagnostics) ---")
            data = driver.get_diagnostics()
            UpdateCounter(data, "get_diagnostics()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            # --- Navigation Data (GET methods) ---
            logger.info("--- Requesting Navigation Data (get_navigation_data) ---")
            data = driver.get_navigation_data()
            UpdateCounter(data, "get_navigation_data()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info("--- Requesting DVL Velocity Data (get_dvl_velocity_data) ---")
            data = driver.get_dvl_velocity_data()
            UpdateCounter(data, "get_dvl_velocity_data()", str_valid, str_timeout)
            time.sleep(COMMAND_PAUSE_SECONDS)

            logger.info(f"========= End of Request Cycle {loop_count}. Waiting {REQUEST_INTERVAL_SECONDS} seconds... =========")
            logger.info("str_valid   "+str(len(str_valid))  +"="+str(str_valid))
            logger.info("str_timeout "+str(len(str_timeout))+"="+str(str_timeout))
            time.sleep(REQUEST_INTERVAL_SECONDS)

    except KeyboardInterrupt:
        logger.info("Keyboard interrupt received. Stopping application...")
    except Exception as e:
        logger.critical(f"An unexpected error occurred during main loop: {e}", exc_info=True)
    finally:
        logger.info("Attempting to disconnect from the device...")
        if 'driver' in locals() and driver and hasattr(driver, 'serial_conn') and driver.serial_conn :
             if hasattr(driver.serial_conn, 'is_open') and driver.serial_conn.is_open:
                driver.disconnect()
                logger.info("Disconnected successfully.")
             else: # Port was not open, but driver object exists
                if hasattr(driver, '_reader_thread') and driver._reader_thread and driver._reader_thread.is_alive():
                    logger.info("Port was not open, ensuring reader thread is stopped if it was started.")
                    driver._stop_event.set()
                    driver._reader_thread.join(timeout=1.0)
        else:
            logger.info("Driver was not connected or instance not fully available for disconnect.")

if __name__ == "__main__":
    main()
    logger.info("Real device interaction script finished.")