"""
 Example demonstrating the synchronous BLE transport for Telemetrix
 on Raspberry Pi Pico 2W

 This example shows how to:
 - Connect to a Pico 2W over BLE
 - Send commands using the Telemetrix protocol
 - Receive and process data from the Pico
"""

import time
import sys
from telemetrix_rpi_pico_2w_ble import TelemetrixRpiPico2wBle


# Telemetrix command constants (from your protocol)
# You'll need to define these based on your actual protocol
class Commands:
    """Telemetrix command opcodes"""
    LOOP_COMMAND = 1
    SET_PIN_MODE = 2
    DIGITAL_WRITE = 3
    ANALOG_WRITE = 4
    # Add more as needed from your protocol


class TelemetrixBleExample:
    """
    Example class showing how to use the synchronous BLE transport
    with Telemetrix protocol
    """

    def __init__(self, device_name="Tmx4Pico2W"):
        """
        Initialize the example

        :param device_name: BLE device name to connect to
        """
        self.device_name = device_name
        self.board = None
        self.receive_buffer = bytearray()

    def data_receive_callback(self, sender, data):
        """
        Callback to handle incoming data from the Pico.
        This is where you'd parse Telemetrix protocol responses.

        :param sender: BLE characteristic that sent the data
        :param data: bytes received
        """
        print(f"Received {len(data)} bytes: {list(data)}")

        # Add to receive buffer
        self.receive_buffer.extend(data)

        # Process complete messages
        # This is a simple example - adjust based on your protocol
        while len(self.receive_buffer) >= 2:  # Assuming 2-byte messages
            # Extract message
            message = bytes(self.receive_buffer[:2])
            self.receive_buffer = self.receive_buffer[2:]

            # Parse message based on Telemetrix protocol
            self.parse_telemetrix_message(message)

    def parse_telemetrix_message(self, message):
        """
        Parse a Telemetrix protocol message

        :param message: bytes to parse
        """
        if len(message) < 1:
            return

        command = message[0]

        # Example parsing - adjust based on your actual protocol
        if command == Commands.LOOP_COMMAND:
            print("Received loop command response")
        else:
            print(f"Received command {command} with data: {list(message[1:])}")

    def connect(self):
        """Connect to the BLE device"""
        print(f"Connecting to {self.device_name}...")

        self.board = TelemetrixRpiPico2wBle(
            self.device_name,
            receive_callback=self.data_receive_callback
        )

        if self.board.connect(timeout=15):
            print("Successfully connected!")
            return True
        else:
            print("Failed to connect")
            return False

    def send_command(self, command_bytes):
        """
        Send a Telemetrix command to the Pico

        :param command_bytes: list of bytes or bytes object to send
        """
        if isinstance(command_bytes, list):
            print(f"Sending command: {command_bytes}")
        else:
            print(f"Sending command: {list(command_bytes)}")

        return self.board.write(command_bytes)

    def disconnect(self):
        """Disconnect from the BLE device"""
        if self.board:
            print("Disconnecting...")
            self.board.disconnect()
            print("Disconnected")

    def run_example(self):
        """Run a simple example sequence"""
        try:
            # Connect
            if not self.connect():
                return

            # Give connection time to stabilize
            time.sleep(1)

            # Example 1: Send a simple 2-byte command
            print("\n=== Example 1: Simple Command ===")
            self.send_command([1, 62])
            time.sleep(1)

            # Example 2: Send loop command (keep-alive)
            print("\n=== Example 2: Loop Command ===")
            self.send_command([Commands.LOOP_COMMAND, 0])
            time.sleep(1)

            # Example 3: Set pin mode (example - adjust for your protocol)
            print("\n=== Example 3: Set Pin Mode ===")
            # Command format might be: [SET_PIN_MODE, pin_number, mode]
            pin = 25  # LED pin on Pico
            mode = 1  # OUTPUT mode
            self.send_command([Commands.SET_PIN_MODE, pin, mode])
            time.sleep(1)

            # Example 4: Digital write
            print("\n=== Example 4: Digital Write ===")
            # Command format might be: [DIGITAL_WRITE, pin_number, value]
            self.send_command([Commands.DIGITAL_WRITE, pin, 1])  # Turn on
            time.sleep(1)

            self.send_command([Commands.DIGITAL_WRITE, pin, 0])  # Turn off
            time.sleep(1)

            # Example 5: Multiple rapid commands
            print("\n=== Example 5: Blink LED ===")
            for i in range(5):
                print(f"Blink {i + 1}/5")
                self.send_command([Commands.DIGITAL_WRITE, pin, 1])
                time.sleep(0.3)
                self.send_command([Commands.DIGITAL_WRITE, pin, 0])
                time.sleep(0.3)

            print("\n=== Example complete ===")

        except KeyboardInterrupt:
            print("\nInterrupted by user")
        except Exception as e:
            print(f"Error: {e}")
            import traceback
            traceback.print_exc()
        finally:
            # Always disconnect
            self.disconnect()


def main():
    """Main entry point"""
    # You can specify a different device name if needed
    device_name = "Tmx4Pico2W"

    if len(sys.argv) > 1:
        device_name = sys.argv[1]

    print("=" * 60)
    print("Telemetrix BLE Example")
    print("=" * 60)
    print(f"Target device: {device_name}")
    print("Press Ctrl+C to exit")
    print("=" * 60)

    example = TelemetrixBleExample(device_name)
    example.run_example()


if __name__ == "__main__":
    main()