#!/usr/bin/env python
	
"""
UART Service
-------------

An example showing how to write a simple program using the Nordic Semiconductor
(nRF) UART service.

"""

import asyncio
import sys
import numpy as np

from bleak import BleakScanner, BleakClient
from bleak.backends.scanner import AdvertisementData
from bleak.backends.device import BLEDevice

wanted_name = u"MED-WATCH"
docsv = True

UART_SERVICE_UUID = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
UART_RX_CHAR_UUID = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
UART_TX_CHAR_UUID = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

# All BLE devices have MTU of at least 23. Subtracting 3 bytes overhead, we can
# safely send 20 bytes at a time to any device supporting this service.
UART_SAFE_SIZE = 20


def mw_packet_decode(thearray):
    if thearray[0] != 36:
        print("data is misaligned!")
        return  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
    if len(thearray) < 24:
        print("packet is too short!")
        return  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
    spo2offset = 11
    ecgoffset = 1
    filteredoffset = 7
    rawoffset =  12
    bpmoffset = 9
    accoffset = 18

    spo2 = int(thearray[spo2offset])

    ecg_1 = 256 * int(thearray[ecgoffset]) + \
            int(thearray[ecgoffset + 1])
    ecg_2 = 256 * int(thearray[ecgoffset + 2]) + \
            int(thearray[ecgoffset + 3])
    ecg_3 = 256 * int(thearray[ecgoffset + 4]) + \
            int(thearray[ecgoffset + 5])

    ir_filtered =   65536 * int(thearray[filteredoffset]) + \
                    256 * int(thearray[filteredoffset + 1]) + \
                    int(thearray[filteredoffset + 2])
    red_filtered =  65536 * int(thearray[filteredoffset + 3]) + \
                    256 * int(thearray[filteredoffset + 4]) + \
                    int(thearray[filteredoffset + 5])

    ir_raw =    65536 * int(thearray[rawoffset]) + \
                256 * int(thearray[rawoffset + 1]) + \
                int(thearray[rawoffset + 2])
    red_raw =   65536 * int(thearray[rawoffset + 3]) + \
                256 * int(thearray[rawoffset + 4]) + \
                int(thearray[rawoffset + 5])

    bpm =   256 * int(thearray[bpmoffset]) + \
            int(thearray[bpmoffset + 1])

    acc_x = 256 * int(thearray[accoffset]) + \
            int(thearray[accoffset + 1])
    acc_y = 256 * int(thearray[accoffset + 2]) + \
            int(thearray[accoffset + 3])
    acc_z = 256 * int(thearray[accoffset + 4]) + \
            int(thearray[accoffset + 5])

    return  spo2, ecg_1, ecg_2, ecg_3, ir_filtered, red_filtered, ir_raw, red_raw, bpm, acc_x, acc_y, acc_z


async def uart_terminal(thewantedname):
    """This is a simple "terminal" program that uses the Nordic Semiconductor
    (nRF) UART service. It reads from stdin and sends each line of data to the
    remote device. Any data received from the device is printed to stdout.
    """

    def match_nus_uuid(device: BLEDevice, adv: AdvertisementData):
        # This assumes that the device includes the UART service UUID in the
        # advertising data. This test may need to be adjusted depending on the
        # actual advertising data supplied by the device.
        if UART_SERVICE_UUID.lower() in adv.service_uuids:
            thisname = str(adv.local_name).encode("utf-8")
            if thisname[0:9] == thewantedname.encode("utf-8"):
                print(f"connected to {str(thisname)}")
                if docsv:
                    print("spo2, bpm, ecg_1, ecg_2, ecg_3, ir_raw, red_raw, ir_filtered, red_filtered, acc_x, acc_y, acc_z")
                return True
        return False


    device = await BleakScanner.find_device_by_filter(match_nus_uuid)

    def handle_disconnect(_: BleakClient):
        print("Device was disconnected, goodbye.")
        # cancelling all tasks effectively ends the program
        for task in asyncio.all_tasks():
            task.cancel()

    def handle_rx(_: int, data: bytearray):
        uint8_values = np.frombuffer(data, dtype=np.uint8)
        spo2, ecg_1, ecg_2, ecg_3, ir_filtered, red_filtered, ir_raw, red_raw, bpm, acc_x, acc_y, acc_z = mw_packet_decode(uint8_values)
        verbose = False
        if verbose:
            print(f"spo2: {spo2}, bpm: {bpm}")
            print(f"ecg: {ecg_1}, {ecg_2}, {ecg_3}")
            print(f"raw (ir, red): {ir_raw}, {red_raw}")
            print(f"filtered (ir, red): {ir_filtered}, {red_filtered}")
            print(f"acc: {acc_x}, {acc_y}, {acc_z}")
        if docsv:
            print(f"{spo2}, {bpm}, {ecg_1}, {ecg_2}, {ecg_3}, {ir_raw}, {red_raw}, {ir_filtered}, {red_filtered}, {acc_x}, {acc_y}, {acc_z}")
        else:
            print("received:", uint8_values)

    async with BleakClient(device, disconnected_callback=handle_disconnect) as client:
        await client.start_notify(UART_TX_CHAR_UUID, handle_rx)

        #print("Connected, start typing and press ENTER...")

        loop = asyncio.get_event_loop()

        while True:
            # This waits until you type a line and press ENTER.
            # A real terminal program might put stdin in raw mode so that things
            # like CTRL+C get passed to the remote device.
            data = await loop.run_in_executor(None, sys.stdin.buffer.readline)

            # data will be empty on EOF (e.g. CTRL+D on *nix)
            if not data:
                break

            # some devices, like devices running MicroPython, expect Windows
            # line endings (uncomment line below if needed)
            # data = data.replace(b"\n", b"\r\n")

            await client.write_gatt_char(UART_RX_CHAR_UUID, data)
            print("sent:", data)


# It is important to use asyncio.run() to get proper cleanup on KeyboardInterrupt.
# This was introduced in Python 3.7. If you need it in Python 3.6, you can copy
# it from https://github.com/python/cpython/blob/3.7/Lib/asyncio/runners.py
try:
    asyncio.run(uart_terminal(wanted_name))
except asyncio.CancelledError:
    # task is cancelled on disconnect, so we ignore this error
    pass
