#!python2-32
from struct import unpack
from py9b.link.base import LinkOpenException, LinkTimeoutException

# from py9b.link.tcp import TCPLink
# from py9b.link.ble import BLELink
from py9b.link.serial import SerialLink
from py9b.transport.base import BaseTransport as BT
from py9b.transport.packet import BasePacket as PKT
from py9b.transport.xiaomi import XiaomiTransport
from py9b.transport.ninebot import NinebotTransport

link = SerialLink()
# link = TCPLink()
# link = BLELink()

with link:
    print("Scanning...")
    ports = link.scan()
    print(ports)

    # tran = XiaomiTransport(link)
    tran = NinebotTransport(link)

    # link.open(("127.0.0.1", 6000))
    link.open(ports[0][1])
    print("Connected")

    last_esc_64 = ""
    last_esc_65 = ""
    last_ble_64 = ""
    try:
        while True:
            try:
                rsp = tran.recv()
                if not rsp:
                    continue
                if rsp.src == BT.HOST and rsp.dst == BT.ESC and rsp.cmd in (0x64, 0x65):
                    if len(rsp.data) == 5:
                        if rsp.data == last_esc_65:
                            continue
                        ll, throttle, brake, u2, u3 = unpack("<BBBBB", rsp.data)
                        print(
                            "BLE->ESC: TH: %02X, BR: %02X, %02X %02X"
                            % (throttle, brake, u2, u3)
                        )
                        last_esc_65 = rsp.data
                        continue
                    elif len(rsp.data) == 7:
                        if rsp.data == last_esc_64:
                            continue
                        ll, throttle, brake, u2, u3, ver = unpack("<BBBBBH", rsp.data)
                        print(
                            "BLE->ESC: TH: %02X, BR: %02X, %02X %02X, VER: %04X"
                            % (throttle, brake, u2, u3, ver)
                        )
                        last_esc_64 = rsp.data
                        continue
                elif rsp.src == BT.HOST and rsp.dst == BT.BLE and rsp.cmd == 0x64:
                    if len(rsp.data) == 4:
                        if rsp.data == last_ble_64:
                            continue
                        u0, u1, u2, u3 = unpack("<BBBB", rsp.data)
                        print("ESC->BLE: %02X %02X %02X %02X" % (u0, u1, u2, u3))
                        last_ble_64 = rsp.data
                        continue
                print(rsp)
            except LinkTimeoutException:
                pass
    except KeyboardInterrupt:
        pass

    link.close()