import serial  # You will need to install pyserial using "pip install pyserial"
import binascii
import argparse
import time

# Please start with command: python F9_Enable_Debug.py --help
# This python script REQUIRES the GGA message to be enabled to work.

# Constants
DEFAULT_BAUD_RATE = 38400
TARGET_BAUD_RATE = 460800

# Hardcoded HEX Commands
ENABLE_DEBUG_OVER_USB = binascii.unhexlify("B5 62 0A 04 00 00 0E 34 B5 62 06 8A 81 00 00 01 00 00 BD 00 91 20 01 C2 00 91 20 01 D6 00 91 20 01 C7 00 91 20 01 AE 00 91 20 01 DB 00 91 20 01 23 04 91 20 01 66 02 91 20 01 0D 04 91 20 01 50 06 91 20 01 16 02 91 20 01 2F 02 91 20 01 01 00 78 10 01 02 00 78 10 01 04 00 78 10 01 52 03 91 20 01 99 01 91 20 01 09 00 91 20 01 90 00 91 20 01 18 00 91 20 01 48 03 91 20 01 1D 00 91 20 01 A7 02 91 20 01 6B 02 91 20 01 08 06 91 20 01 10 B4 B5 62 06 8A 09 00 00 01 00 00 B9 06 91 20 01 0B DA".replace(" ", ""))
ENABLE_DEBUG_OVER_UART = binascii.unhexlify("B5 62 0A 04 00 00 0E 34 B5 62 06 8A 81 00 00 01 00 00 01 00 74 10 01 02 00 74 10 01 04 00 74 10 01 BB 00 91 20 01 C0 00 91 20 01 D4 00 91 20 01 C5 00 91 20 01 AC 00 91 20 01 D9 00 91 20 01 21 04 91 20 01 64 02 91 20 01 0B 04 91 20 01 4E 06 91 20 01 14 02 91 20 01 2D 02 91 20 01 50 03 91 20 01 97 01 91 20 01 07 00 91 20 01 8E 00 91 20 01 16 00 91 20 01 46 03 91 20 01 1B 00 91 20 01 A5 02 91 20 01 69 02 91 20 01 06 06 91 20 01 D8 BE B5 62 06 8A 09 00 00 01 00 00 B7 06 91 20 01 09 D0".replace(" ", ""))
CHANGE_UART_BAUDRATE_460800 = binascii.unhexlify("B5 62 06 8A 0C 00 00 01 00 00 01 00 52 40 00 08 07 00 3F 83".replace(" ", ""))
ENABLE_GGA_OVER_UART_USB = binascii.unhexlify("B5 62 06 8A 18 00 00 01 00 00 02 00 74 10 01 02 00 78 10 01 BB 00 91 20 01 BD 00 91 20 01 97 8F".replace(" ", ""))

# ---------------------------- Argument Parsing ---------------------------------------

parser = argparse.ArgumentParser(
    description="Enable debug messages on F9 module via USB or UART connection at 460800 baudrate. Specify port (-P) and the script handles the rest.",
    epilog="Examples:\n"
           "  python F9_Enable_Debug.py -P /dev/ttyUSB0\n"
           "  python F9_Enable_Debug.py -P COM01",
    formatter_class=argparse.RawTextHelpFormatter)

parser.add_argument("-P", "--Port", required=True, help="Specify the port name, e.g., 'COM01' or '/dev/ttyUSB0'. (required)")

# Parse the arguments
args = parser.parse_args()

# ---------------------------- Command Execution ---------------------------------------

def detect_nmea(ser, timeout=2):
    """Detects NMEA message within the specified timeout period."""
    start_time = time.time()
    while time.time() - start_time < timeout:
        response = ser.read(500)
        if b"$GP" in response or b"$GN" in response:
            return True
    return False

def open_port(port, baudrate, timeout=1):
    """Opens and returns a serial port at the specified baud rate."""
    try:
        ser = serial.Serial(port, baudrate, timeout=timeout)
        ser.reset_input_buffer()
        return ser
    except serial.SerialException as e:
        print(f"Failed to open port {port} at baudrate {baudrate}: {e}")
        raise

def send_command(ser, command, message, verbose=False):
    ser.write(command)
    if verbose:
        print(message)


ser = None # needed for finnaly check
try:
    # Initial check at 38400 to enable GGA and to detect any connection
    print("Attempting to open port at baud rate 38400 to detect initial connection...")
    ser = open_port(args.Port, DEFAULT_BAUD_RATE)
    send_command(ser, ENABLE_GGA_OVER_UART_USB, "Enable GGA messages over USB & UART", verbose=True)
    
    if detect_nmea(ser):
        print("NMEA message detected at 38400. Checking 460800 to confirm USB or UART port...")
        ser.close()

        # Check at 460800 to confirm USB
        ser = open_port(args.Port, TARGET_BAUD_RATE)
        if detect_nmea(ser):
            print("NMEA message also detected at 460800. USB connection confirmed.")
            send_command(ser, ENABLE_DEBUG_OVER_USB, "Enable Debug messages over USB", verbose=True) 
        
        # If no NMEA message detected at 460800, assume it's UART and return to 38400 to change baud rate
        else:
            print("No NMEA message detected at 460800. UART connection confirmed, returning to 38400 to increase baud rate...")
            ser.close()
            
            # Reopen at 38400 to send the change baud rate command
            print("Changing UART baudrate to 460800")
            ser = open_port(args.Port, DEFAULT_BAUD_RATE)
            send_command(ser, CHANGE_UART_BAUDRATE_460800, "Change UART Baud Rate to 460800", verbose=True)
            ser.close()

            # Reopen at 460800 to send UART debug message command
            print("Enabling Debug messages over UART port")
            ser = open_port(args.Port, TARGET_BAUD_RATE)
            send_command(ser, ENABLE_DEBUG_OVER_UART, "Enable Debug messages over UART", verbose=True)

    # If no NMEA detected at 38400, assume UART and starting Autobaud
    else:
        print("No NMEA message detected at 38400. Possible UART connection. Autobauding initiated...")
        ser.close()
        for baudrate in [9600, 57600, 115200, 230400, 460800, 921600]:
            print(f"Attempting to open UART port at baud rate {baudrate}")
            ser = open_port(args.Port, baudrate)
            send_command(ser, ENABLE_GGA_OVER_UART_USB, "Enable GGA") # Attempt to enable GGA before checking for NMEA messages at each baud rate
            
            if detect_nmea(ser):
                print(f"NMEA message detected at baud rate {baudrate}. UART connection confirmed.")
                send_command(ser, CHANGE_UART_BAUDRATE_460800, "Change UART Baud Rate to 460800", verbose=True)
                ser.close()
                
                # Reopen at 460800 to send UART debug command
                ser = open_port(args.Port, TARGET_BAUD_RATE)
                send_command(ser, ENABLE_DEBUG_OVER_UART, "Enable Debug messages over UART", verbose=True)
                break
            ser.close()
        else:
            print("Autobauding failed. Could not establish a connection at any of the baud rates.")

except KeyboardInterrupt:
    print("\nProcess interrupted by user.")
except serial.SerialException as e:
    print(f"Error opening serial port, please check your port is not in use: {e}")
except Exception as e:
    print(f"Unexpected error: {e}")
finally:
    # Ensure serial port is closed
    if ser and ser.is_open:
        ser.close()
        print("Serial port closed.")