#!/usr/bin/env python3

import argparse
import binascii
import time

from .btle import UUID, Peripheral, ADDR_TYPE_RANDOM, DefaultDelegate


def write_uint16(data, value, index):
    """Write 16bit value into data string at index and return new string"""
    data = data.decode("utf-8")  # This line is added to make sure both Python 2 and 3 works
    return f"{data[: index * 4]}{value & 0xFF:02x}{value >> 8:02x}{data[index * 4 + 4 :]}"


def write_uint8(data, value, index):
    """Write 8bit value into data string at index and return new string"""
    data = data.decode("utf-8")  # This line is added to make sure both Python 2 and 3 works
    return f"{data[: index * 2]}{value:02x}{data[index * 2 + 2 :]}"


# Please see # Ref https://nordicsemiconductor.github.io/Nordic-Thingy52-FW/documentation
# for more information on the UUIDs of the Services and Characteristics that are being used
def Nordic_UUID(val):
    """Adds base UUID and inserts value to return Nordic UUID"""
    return UUID(f"EF68{val:04X}-9B35-4933-9B10-52FFA9740042")


# Definition of all UUID used by Thingy
CCCD_UUID = 0x2902

BATTERY_SERVICE_UUID = 0x180F
BATTERY_LEVEL_UUID = 0x2A19

ENVIRONMENT_SERVICE_UUID = 0x0200
E_TEMPERATURE_CHAR_UUID = 0x0201
E_PRESSURE_CHAR_UUID = 0x0202
E_HUMIDITY_CHAR_UUID = 0x0203
E_GAS_CHAR_UUID = 0x0204
E_COLOR_CHAR_UUID = 0x0205
E_CONFIG_CHAR_UUID = 0x0206

USER_INTERFACE_SERVICE_UUID = 0x0300
UI_LED_CHAR_UUID = 0x0301
UI_BUTTON_CHAR_UUID = 0x0302
UI_EXT_PIN_CHAR_UUID = 0x0303

MOTION_SERVICE_UUID = 0x0400
M_CONFIG_CHAR_UUID = 0x0401
M_TAP_CHAR_UUID = 0x0402
M_ORIENTATION_CHAR_UUID = 0x0403
M_QUATERNION_CHAR_UUID = 0x0404
M_STEP_COUNTER_UUID = 0x0405
M_RAW_DATA_CHAR_UUID = 0x0406
M_EULER_CHAR_UUID = 0x0407
M_ROTATION_MATRIX_CHAR_UUID = 0x0408
M_HEAIDNG_CHAR_UUID = 0x0409
M_GRAVITY_VECTOR_CHAR_UUID = 0x040A

SOUND_SERVICE_UUID = 0x0500
S_CONFIG_CHAR_UUID = 0x0501
S_SPEAKER_DATA_CHAR_UUID = 0x0502
S_SPEAKER_STATUS_CHAR_UUID = 0x0503
S_MICROPHONE_CHAR_UUID = 0x0504

# Notification handles used in notification delegate
e_temperature_handle = None
e_pressure_handle = None
e_humidity_handle = None
e_gas_handle = None
e_color_handle = None
ui_button_handle = None
m_tap_handle = None
m_orient_handle = None
m_quaternion_handle = None
m_stepcnt_handle = None
m_rawdata_handle = None
m_euler_handle = None
m_rotation_handle = None
m_heading_handle = None
m_gravity_handle = None
s_speaker_status_handle = None
s_microphone_handle = None


class BatterySensor:
    """
    Battery Service module. Instance the class and enable to get access to Battery interface.
    """

    svcUUID = UUID(BATTERY_SERVICE_UUID)  # Ref https://www.bluetooth.com/specifications/gatt/services
    dataUUID = UUID(BATTERY_LEVEL_UUID)  # Ref https://www.bluetooth.com/specifications/gatt/characteristics

    def __init__(self, periph):
        self.periph = periph
        self.service = None
        self.data = None

    def enable(self):
        """Enables the class by finding the service and its characteristics."""
        if self.service is None:
            self.service = self.periph.getServiceByUUID(self.svcUUID)
        if self.data is None:
            self.data = self.service.getCharacteristics(self.dataUUID)[0]

    def read(self):
        """Returns the battery level in percent"""
        val = ord(self.data.read())
        return val


class EnvironmentService:
    """
    Environment service module. Instance the class and enable to get access to the Environment interface.
    """

    serviceUUID = Nordic_UUID(ENVIRONMENT_SERVICE_UUID)
    temperature_char_uuid = Nordic_UUID(E_TEMPERATURE_CHAR_UUID)
    pressure_char_uuid = Nordic_UUID(E_PRESSURE_CHAR_UUID)
    humidity_char_uuid = Nordic_UUID(E_HUMIDITY_CHAR_UUID)
    gas_char_uuid = Nordic_UUID(E_GAS_CHAR_UUID)
    color_char_uuid = Nordic_UUID(E_COLOR_CHAR_UUID)
    config_char_uuid = Nordic_UUID(E_CONFIG_CHAR_UUID)

    def __init__(self, periph):
        self.periph = periph
        self.environment_service = None
        self.temperature_char = None
        self.temperature_cccd = None
        self.pressure_char = None
        self.pressure_cccd = None
        self.humidity_char = None
        self.humidity_cccd = None
        self.gas_char = None
        self.gas_cccd = None
        self.color_char = None
        self.color_cccd = None
        self.config_char = None

    def enable(self):
        """Enables the class by finding the service and its characteristics."""
        global e_temperature_handle
        global e_pressure_handle
        global e_humidity_handle
        global e_gas_handle
        global e_color_handle

        if self.environment_service is None:
            self.environment_service = self.periph.getServiceByUUID(self.serviceUUID)
        if self.temperature_char is None:
            self.temperature_char = self.environment_service.getCharacteristics(self.temperature_char_uuid)[0]
            e_temperature_handle = self.temperature_char.getHandle()
            self.temperature_cccd = self.temperature_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.pressure_char is None:
            self.pressure_char = self.environment_service.getCharacteristics(self.pressure_char_uuid)[0]
            e_pressure_handle = self.pressure_char.getHandle()
            self.pressure_cccd = self.pressure_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.humidity_char is None:
            self.humidity_char = self.environment_service.getCharacteristics(self.humidity_char_uuid)[0]
            e_humidity_handle = self.humidity_char.getHandle()
            self.humidity_cccd = self.humidity_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.gas_char is None:
            self.gas_char = self.environment_service.getCharacteristics(self.gas_char_uuid)[0]
            e_gas_handle = self.gas_char.getHandle()
            self.gas_cccd = self.gas_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.color_char is None:
            self.color_char = self.environment_service.getCharacteristics(self.color_char_uuid)[0]
            e_color_handle = self.color_char.getHandle()
            self.color_cccd = self.color_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.config_char is None:
            self.config_char = self.environment_service.getCharacteristics(self.config_char_uuid)[0]

    def set_temperature_notification(self, state):
        if self.temperature_cccd is not None:
            if state is True:
                self.temperature_cccd.write(b"\x01\x00", True)
            else:
                self.temperature_cccd.write(b"\x00\x00", True)

    def set_pressure_notification(self, state):
        if self.pressure_cccd is not None:
            if state is True:
                self.pressure_cccd.write(b"\x01\x00", True)
            else:
                self.pressure_cccd.write(b"\x00\x00", True)

    def set_humidity_notification(self, state):
        if self.humidity_cccd is not None:
            if state is True:
                self.humidity_cccd.write(b"\x01\x00", True)
            else:
                self.humidity_cccd.write(b"\x00\x00", True)

    def set_gas_notification(self, state):
        if self.gas_cccd is not None:
            if state is True:
                self.gas_cccd.write(b"\x01\x00", True)
            else:
                self.gas_cccd.write(b"\x00\x00", True)

    def set_color_notification(self, state):
        if self.color_cccd is not None:
            if state is True:
                self.color_cccd.write(b"\x01\x00", True)
            else:
                self.color_cccd.write(b"\x00\x00", True)

    # fmt: off
    def configure(self, temp_int=None, press_int=None, humid_int=None, gas_mode_int=None, color_int=None, color_sens_calib=None,):
        # fmt: on
        if self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            if temp_int is not None:
                new_config = write_uint16(current_config, temp_int, 0)
            if press_int is not None:
                new_config = write_uint16(current_config, press_int, 1)
            if humid_int is not None:
                new_config = write_uint16(current_config, humid_int, 2)
            if gas_mode_int is not None:
                new_config = write_uint8(current_config, gas_mode_int, 8)
            if color_int is not None:
                new_config = write_uint16(current_config, color_int, 3)
            if color_sens_calib is not None:
                new_config = write_uint8(current_config, color_sens_calib[0], 9)
                new_config = write_uint8(current_config, color_sens_calib[1], 10)
                new_config = write_uint8(current_config, color_sens_calib[2], 11)
            self.config_char.write(binascii.a2b_hex(new_config), True)

    def disable(self):
        self.set_temperature_notification(False)
        self.set_pressure_notification(False)
        self.set_humidity_notification(False)
        self.set_gas_notification(False)
        self.set_color_notification(False)


class UserInterfaceService:
    """
    User interface service module. Instance the class and enable to get access to the UI interface.
    """

    serviceUUID = Nordic_UUID(USER_INTERFACE_SERVICE_UUID)
    led_char_uuid = Nordic_UUID(UI_LED_CHAR_UUID)
    btn_char_uuid = Nordic_UUID(UI_BUTTON_CHAR_UUID)

    # To be added: EXT PIN CHAR

    def __init__(self, periph):
        self.periph = periph
        self.ui_service = None
        self.led_char = None
        self.btn_char = None
        self.btn_char_cccd = None  # To be added: EXT PIN CHAR

    def enable(self):
        """Enables the class by finding the service and its characteristics."""
        global ui_button_handle

        if self.ui_service is None:
            self.ui_service = self.periph.getServiceByUUID(self.serviceUUID)
        if self.led_char is None:
            self.led_char = self.ui_service.getCharacteristics(self.led_char_uuid)[0]
        if self.btn_char is None:
            self.btn_char = self.ui_service.getCharacteristics(self.btn_char_uuid)[0]
            ui_button_handle = self.btn_char.getHandle()
            self.btn_char_cccd = self.btn_char.getDescriptors(forUUID=CCCD_UUID)[0]

    def set_led_mode_off(self):
        self.led_char.write(b"\x00", True)

    def set_led_mode_constant(self, r, g, b):
        teptep = f"01{r:02X}{g:02X}{b:02X}"
        self.led_char.write(binascii.a2b_hex(teptep), True)

    def set_led_mode_breathe(self, color, intensity, delay):
        """
        Set LED to breathe mode.
        color has to be within 0x01 and 0x07
        intensity [%] has to be within 1-100
        delay [ms] has to be within 1 ms - 10 s
        """
        teptep = f"02{color:02X}{intensity:02X}{delay & 0xFF:02X}{delay >> 8:02X}"
        self.led_char.write(binascii.a2b_hex(teptep), True)

    def set_led_mode_one_shot(self, color, intensity):
        """
        Set LED to one shot mode.
        color has to be within 0x01 and 0x07
        intensity [%] has to be within 1-100
        """
        teptep = "03{color:02X}{intensity:02X}"
        self.led_char.write(binascii.a2b_hex(teptep), True)

    def set_btn_notification(self, state):
        if self.btn_char_cccd is not None:
            if state is True:
                self.btn_char_cccd.write(b"\x01\x00", True)
            else:
                self.btn_char_cccd.write(b"\x00\x00", True)

    def disable(self):
        self.set_btn_notification(False)


class MotionService:
    """
    Motion service module. Instance the class and enable to get access to the Motion interface.
    """

    serviceUUID = Nordic_UUID(MOTION_SERVICE_UUID)
    config_char_uuid = Nordic_UUID(M_CONFIG_CHAR_UUID)
    tap_char_uuid = Nordic_UUID(M_TAP_CHAR_UUID)
    orient_char_uuid = Nordic_UUID(M_ORIENTATION_CHAR_UUID)
    quaternion_char_uuid = Nordic_UUID(M_QUATERNION_CHAR_UUID)
    stepcnt_char_uuid = Nordic_UUID(M_STEP_COUNTER_UUID)
    rawdata_char_uuid = Nordic_UUID(M_RAW_DATA_CHAR_UUID)
    euler_char_uuid = Nordic_UUID(M_EULER_CHAR_UUID)
    rotation_char_uuid = Nordic_UUID(M_ROTATION_MATRIX_CHAR_UUID)
    heading_char_uuid = Nordic_UUID(M_HEAIDNG_CHAR_UUID)
    gravity_char_uuid = Nordic_UUID(M_GRAVITY_VECTOR_CHAR_UUID)

    def __init__(self, periph):
        self.periph = periph
        self.motion_service = None
        self.config_char = None
        self.tap_char = None
        self.tap_char_cccd = None
        self.orient_char = None
        self.orient_cccd = None
        self.quaternion_char = None
        self.quaternion_cccd = None
        self.stepcnt_char = None
        self.stepcnt_cccd = None
        self.rawdata_char = None
        self.rawdata_cccd = None
        self.euler_char = None
        self.euler_cccd = None
        self.rotation_char = None
        self.rotation_cccd = None
        self.heading_char = None
        self.heading_cccd = None
        self.gravity_char = None
        self.gravity_cccd = None

    def enable(self):
        """Enables the class by finding the service and its characteristics."""
        global m_tap_handle
        global m_orient_handle
        global m_quaternion_handle
        global m_stepcnt_handle
        global m_rawdata_handle
        global m_euler_handle
        global m_rotation_handle
        global m_heading_handle
        global m_gravity_handle

        if self.motion_service is None:
            self.motion_service = self.periph.getServiceByUUID(self.serviceUUID)
        if self.config_char is None:
            self.config_char = self.motion_service.getCharacteristics(self.config_char_uuid)[0]
        if self.tap_char is None:
            self.tap_char = self.motion_service.getCharacteristics(self.tap_char_uuid)[0]
            m_tap_handle = self.tap_char.getHandle()
            self.tap_char_cccd = self.tap_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.orient_char is None:
            self.orient_char = self.motion_service.getCharacteristics(self.orient_char_uuid)[0]
            m_orient_handle = self.orient_char.getHandle()
            self.orient_cccd = self.orient_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.quaternion_char is None:
            self.quaternion_char = self.motion_service.getCharacteristics(self.quaternion_char_uuid)[0]
            m_quaternion_handle = self.quaternion_char.getHandle()
            self.quaternion_cccd = self.quaternion_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.stepcnt_char is None:
            self.stepcnt_char = self.motion_service.getCharacteristics(self.stepcnt_char_uuid)[0]
            m_stepcnt_handle = self.stepcnt_char.getHandle()
            self.stepcnt_cccd = self.stepcnt_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.rawdata_char is None:
            self.rawdata_char = self.motion_service.getCharacteristics(self.rawdata_char_uuid)[0]
            m_rawdata_handle = self.rawdata_char.getHandle()
            self.rawdata_cccd = self.rawdata_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.euler_char is None:
            self.euler_char = self.motion_service.getCharacteristics(self.euler_char_uuid)[0]
            m_euler_handle = self.euler_char.getHandle()
            self.euler_cccd = self.euler_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.rotation_char is None:
            self.rotation_char = self.motion_service.getCharacteristics(self.rotation_char_uuid)[0]
            m_rotation_handle = self.rotation_char.getHandle()
            self.rotation_cccd = self.rotation_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.heading_char is None:
            self.heading_char = self.motion_service.getCharacteristics(self.heading_char_uuid)[0]
            m_heading_handle = self.heading_char.getHandle()
            self.heading_cccd = self.heading_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.gravity_char is None:
            self.gravity_char = self.motion_service.getCharacteristics(self.gravity_char_uuid)[0]
            m_gravity_handle = self.gravity_char.getHandle()
            self.gravity_cccd = self.gravity_char.getDescriptors(forUUID=CCCD_UUID)[0]

    def set_tap_notification(self, state):
        if self.tap_char_cccd is not None:
            if state is True:
                self.tap_char_cccd.write(b"\x01\x00", True)
            else:
                self.tap_char_cccd.write(b"\x00\x00", True)

    def set_orient_notification(self, state):
        if self.orient_cccd is not None:
            if state is True:
                self.orient_cccd.write(b"\x01\x00", True)
            else:
                self.orient_cccd.write(b"\x00\x00", True)

    def set_quaternion_notification(self, state):
        if self.quaternion_cccd is not None:
            if state is True:
                self.quaternion_cccd.write(b"\x01\x00", True)
            else:
                self.quaternion_cccd.write(b"\x00\x00", True)

    def set_stepcnt_notification(self, state):
        if self.stepcnt_cccd is not None:
            if state is True:
                self.stepcnt_cccd.write(b"\x01\x00", True)
            else:
                self.stepcnt_cccd.write(b"\x00\x00", True)

    def set_rawdata_notification(self, state):
        if self.rawdata_cccd is not None:
            if state is True:
                self.rawdata_cccd.write(b"\x01\x00", True)
            else:
                self.rawdata_cccd.write(b"\x00\x00", True)

    def set_euler_notification(self, state):
        if self.euler_cccd is not None:
            if state is True:
                self.euler_cccd.write(b"\x01\x00", True)
            else:
                self.euler_cccd.write(b"\x00\x00", True)

    def set_rotation_notification(self, state):
        if self.rotation_cccd is not None:
            if state is True:
                self.rotation_cccd.write(b"\x01\x00", True)
            else:
                self.rotation_cccd.write(b"\x00\x00", True)

    def set_heading_notification(self, state):
        if self.heading_cccd is not None:
            if state is True:
                self.heading_cccd.write(b"\x01\x00", True)
            else:
                self.heading_cccd.write(b"\x00\x00", True)

    def set_gravity_notification(self, state):
        if self.gravity_cccd is not None:
            if state is True:
                self.gravity_cccd.write(b"\x01\x00", True)
            else:
                self.gravity_cccd.write(b"\x00\x00", True)

    def configure(
        self,
        step_int=None,
        temp_comp_int=None,
        magnet_comp_int=None,
        motion_freq=None,
        wake_on_motion=None,
    ):
        if step_int is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint16(current_config, step_int, 0)
            self.config_char.write(binascii.a2b_hex(new_config), True)
        if temp_comp_int is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint16(current_config, temp_comp_int, 1)
            self.config_char.write(binascii.a2b_hex(new_config), True)
        if magnet_comp_int is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint16(current_config, magnet_comp_int, 2)
            self.config_char.write(binascii.a2b_hex(new_config), True)
        if motion_freq is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint16(current_config, motion_freq, 3)
            self.config_char.write(binascii.a2b_hex(new_config), True)
        if wake_on_motion is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint8(current_config, wake_on_motion, 8)
            self.config_char.write(binascii.a2b_hex(new_config), True)

    def disable(self):
        self.set_tap_notification(False)
        self.set_orient_notification(False)
        self.set_quaternion_notification(False)
        self.set_stepcnt_notification(False)
        self.set_rawdata_notification(False)
        self.set_euler_notification(False)
        self.set_rotation_notification(False)
        self.set_heading_notification(False)
        self.set_gravity_notification(False)


class SoundService:
    """
    Sound service module. Instance the class and enable to get access to the Sound interface.
    """

    serviceUUID = Nordic_UUID(SOUND_SERVICE_UUID)
    config_char_uuid = Nordic_UUID(S_CONFIG_CHAR_UUID)
    speaker_data_char_uuid = Nordic_UUID(S_SPEAKER_DATA_CHAR_UUID)
    speaker_status_char_uuid = Nordic_UUID(S_SPEAKER_STATUS_CHAR_UUID)
    microphone_char_uuid = Nordic_UUID(S_MICROPHONE_CHAR_UUID)

    def __init__(self, periph):
        self.periph = periph
        self.sound_service = None
        self.config_char = None
        self.speaker_data_char = None
        self.speaker_status_char = None
        self.speaker_status_char_cccd = None
        self.microphone_char = None
        self.microphone_char_cccd = None

    def enable(self):
        """Enables the class by finding the service and its characteristics."""
        global s_speaker_status_handle
        global s_microphone_handle

        if self.sound_service is None:
            self.sound_service = self.periph.getServiceByUUID(self.serviceUUID)
        if self.config_char is None:
            self.config_char = self.sound_service.getCharacteristics(self.config_char_uuid)[0]
        if self.speaker_data_char is None:
            self.speaker_data_char = self.sound_service.getCharacteristics(self.speaker_data_char_uuid)[0]
        if self.speaker_status_char is None:
            self.speaker_status_char = self.sound_service.getCharacteristics(self.speaker_status_char_uuid)[0]
            s_speaker_status_handle = self.speaker_status_char.getHandle()
            self.speaker_status_char_cccd = self.speaker_status_char.getDescriptors(forUUID=CCCD_UUID)[0]
        if self.microphone_char is None:
            self.microphone_char = self.sound_service.getCharacteristics(self.microphone_char_uuid)[0]
            s_microphone_handle = self.microphone_char.getHandle()
            self.microphone_char_cccd = self.microphone_char.getDescriptors(forUUID=CCCD_UUID)[0]

    def play_speaker_sample(self, sample=0):
        if self.speaker_data_char is not None:
            sample_str = "f{sample:02X}"
            self.speaker_data_char.write(binascii.a2b_hex(sample_str), False)

    def set_speaker_status_notification(self, state):
        if self.speaker_status_char_cccd is not None:
            if state is True:
                self.speaker_status_char_cccd.write(b"\x01\x00", True)
            else:
                self.speaker_status_char_cccd.write(b"\x00\x00", True)

    def set_microphone_notification(self, state):
        if self.microphone_char_cccd is not None:
            if state is True:
                self.microphone_char_cccd.write(b"\x01\x00", True)
            else:
                self.microphone_char_cccd.write(b"\x00\x00", True)

    def configure(self, speaker_mode=None, microphone_mode=None):
        if speaker_mode is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint8(current_config, speaker_mode, 0)
            self.config_char.write(binascii.a2b_hex(new_config), True)
        if microphone_mode is not None and self.config_char is not None:
            current_config = binascii.b2a_hex(self.config_char.read())
            new_config = write_uint8(current_config, microphone_mode, 1)
            self.config_char.write(binascii.a2b_hex(new_config), True)

    def disable(self):
        self.set_speaker_status_notification(False)
        self.set_microphone_notification(False)


class MyDelegate(DefaultDelegate):
    def handleNotification(self, hnd, data):
        # Debug print repr(data)
        if hnd == e_temperature_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Temp received:  {self._str_to_int(teptep[:-2])}.{int(teptep[-2:], 16)} degCelsius")

        elif hnd == e_pressure_handle:
            pressure_int, pressure_dec = self._extract_pressure_data(data)
            print(f"Notification: Press received: {pressure_int}.{pressure_dec} hPa")

        elif hnd == e_humidity_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Humidity received: {self._str_to_int(teptep)} %")

        elif hnd == e_gas_handle:
            eco2, tvoc = self._extract_gas_data(data)
            print(f"Notification: Gas received: eCO2 ppm: {eco2}, TVOC ppb: {tvoc} %")

        elif hnd == e_color_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Color: {teptep}")

        elif hnd == ui_button_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Button state [1 -> released]: {self._str_to_int(teptep)}")

        elif hnd == m_tap_handle:
            direction, count = self._extract_tap_data(data)
            print(f"Notification: Tap: direction: {direction}, count: {self._str_to_int(count)}")

        elif hnd == m_orient_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Orient: {teptep}")

        elif hnd == m_quaternion_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Quaternion: {teptep}")

        elif hnd == m_stepcnt_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Step Count: {teptep}")

        elif hnd == m_rawdata_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Raw data: {teptep}")

        elif hnd == m_euler_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Euler: {teptep}")

        elif hnd == m_rotation_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Rotation matrix: {teptep}")

        elif hnd == m_heading_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Heading: {teptep}")

        elif hnd == m_gravity_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Gravity: {teptep}")

        elif hnd == s_speaker_status_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Speaker Status: {teptep}")

        elif hnd == s_microphone_handle:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: Microphone: {teptep}")

        else:
            teptep = binascii.b2a_hex(data)
            print(f"Notification: UNKOWN: hnd {hnd}, data {teptep}")

    def _str_to_int(self, s):
        """Transform hex str into int."""
        i = int(s, 16)
        if i >= 2**7:
            i -= 2**8
        return i

    def _extract_pressure_data(self, data):
        """Extract pressure data from data string."""
        teptep = binascii.b2a_hex(data)
        pressure_int = 0
        for i in range(0, 4):
            pressure_int += int(teptep[i * 2 : (i * 2) + 2], 16) << 8 * i
        pressure_dec = int(teptep[-2:], 16)
        return (pressure_int, pressure_dec)

    def _extract_gas_data(self, data):
        """Extract gas data from data string."""
        teptep = binascii.b2a_hex(data)
        eco2 = int(teptep[:2], 16) + (int(teptep[2:4], 16) << 8)
        tvoc = int(teptep[4:6], 16) + (int(teptep[6:8], 16) << 8)
        return eco2, tvoc

    def _extract_tap_data(self, data):
        """Extract tap data from data string."""
        teptep = binascii.b2a_hex(data)
        direction = teptep[0:2]
        count = teptep[2:4]
        return (direction, count)


class Thingy52(Peripheral):
    """
    Thingy:52 module. Instance the class and enable to get access to the Thingy:52 Sensors.
    The addr of your device has to be know, or can be found by using the hcitool command line
    tool, for example. Call "> sudo hcitool lescan" and your Thingy's address should show up.
    """

    def __init__(self, addr):
        Peripheral.__init__(self, addr, addrType=ADDR_TYPE_RANDOM)

        # Thingy configuration service not implemented
        self.battery = BatterySensor(self)
        self.environment = EnvironmentService(self)
        self.ui = UserInterfaceService(self)
        self.motion = MotionService(self)
        self.sound = SoundService(self)  # DFU Service not implemented


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("mac_address", action="store", help="MAC address of BLE peripheral")
    parser.add_argument(
        "-n",
        action="store",
        dest="count",
        default=0,
        type=int,
        help="Number of times to loop data",
    )
    parser.add_argument("-t", action="store", type=float, default=2.0, help="time between polling")
    parser.add_argument("--temperature", action="store_true", default=False)
    parser.add_argument("--pressure", action="store_true", default=False)
    parser.add_argument("--humidity", action="store_true", default=False)
    parser.add_argument("--gas", action="store_true", default=False)
    parser.add_argument("--color", action="store_true", default=False)
    parser.add_argument("--keypress", action="store_true", default=False)
    parser.add_argument("--tap", action="store_true", default=False)
    parser.add_argument("--orientation", action="store_true", default=False)
    parser.add_argument("--quaternion", action="store_true", default=False)
    parser.add_argument("--stepcnt", action="store_true", default=False)
    parser.add_argument("--rawdata", action="store_true", default=False)
    parser.add_argument("--euler", action="store_true", default=False)
    parser.add_argument("--rotation", action="store_true", default=False)
    parser.add_argument("--heading", action="store_true", default=False)
    parser.add_argument("--gravity", action="store_true", default=False)
    parser.add_argument("--battery", action="store_true", default=False)
    parser.add_argument("--speaker", action="store_true", default=False)
    parser.add_argument("--microphone", action="store_true", default=False)
    args = parser.parse_args()

    print(f"Connecting to {args.mac_address}")
    thingy = Thingy52(args.mac_address)
    print("Connected...")
    thingy.setDelegate(MyDelegate())

    try:
        # Set LED so that we know we are connected
        thingy.ui.enable()
        thingy.ui.set_led_mode_breathe(0x01, 50, 100)  # 0x01 = RED
        print("LED set to breathe mode...")

        # Enabling selected sensors
        print("Enabling selected sensors...")
        # Environment Service
        if args.temperature:
            thingy.environment.enable()
            thingy.environment.configure(temp_int=1000)
            thingy.environment.set_temperature_notification(True)
        if args.pressure:
            thingy.environment.enable()
            thingy.environment.configure(press_int=1000)
            thingy.environment.set_pressure_notification(True)
        if args.humidity:
            thingy.environment.enable()
            thingy.environment.configure(humid_int=1000)
            thingy.environment.set_humidity_notification(True)
        if args.gas:
            thingy.environment.enable()
            thingy.environment.configure(gas_mode_int=1)
            thingy.environment.set_gas_notification(True)
        if args.color:
            thingy.environment.enable()
            thingy.environment.configure(color_int=1000)
            thingy.environment.configure(color_sens_calib=[0, 0, 0])
            thingy.environment.set_color_notification(True)
        # User Interface Service
        if args.keypress:
            thingy.ui.enable()
            thingy.ui.set_btn_notification(True)
        if args.battery:
            thingy.battery.enable()
        # Motion Service
        if args.tap:
            thingy.motion.enable()
            thingy.motion.configure(motion_freq=200)
            thingy.motion.set_tap_notification(True)
        if args.orientation:
            thingy.motion.enable()
            thingy.motion.set_orient_notification(True)
        if args.quaternion:
            thingy.motion.enable()
            thingy.motion.set_quaternion_notification(True)
        if args.stepcnt:
            thingy.motion.enable()
            thingy.motion.configure(step_int=100)
            thingy.motion.set_stepcnt_notification(True)
        if args.rawdata:
            thingy.motion.enable()
            thingy.motion.set_rawdata_notification(True)
        if args.euler:
            thingy.motion.enable()
            thingy.motion.set_euler_notification(True)
        if args.rotation:
            thingy.motion.enable()
            thingy.motion.set_rotation_notification(True)
        if args.heading:
            thingy.motion.enable()
            thingy.motion.set_heading_notification(True)
        if args.gravity:
            thingy.motion.enable()
            thingy.motion.set_gravity_notification(True)
        # Sound Service
        if args.speaker:
            thingy.sound.enable()
            thingy.sound.configure(speaker_mode=0x03)
            thingy.sound.set_speaker_status_notification(True)
            # Test speaker
            thingy.sound.play_speaker_sample(1)
        if args.microphone:
            thingy.sound.enable()
            thingy.sound.configure(microphone_mode=0x01)
            thingy.sound.set_microphone_notification(True)

        # Allow sensors time to start up (might need more time for some sensors to be ready)
        print("All requested sensors and notifications are enabled...")
        time.sleep(1.0)

        counter = 1
        while True:
            if args.battery:
                print("Battery: ", thingy.battery.read())

            if counter >= args.count:
                break

            counter += 1
            thingy.waitForNotifications(args.t)

    finally:
        thingy.disconnect()
        del thingy


if __name__ == "__main__":
    main()
