from bluepy.btle import Peripheral, UUID, DefaultDelegate, BTLEException
from pyparrot.utils.colorPrint import color_print
import struct
import time
from pyparrot.commandsandsensors.DroneSensorParser import get_data_format_and_size
from datetime import datetime

class MinidroneDelegate(DefaultDelegate):
    """
    Handle BLE notififications
    """
    def __init__(self, handle_map, minidrone, ble_connection):
        DefaultDelegate.__init__(self)
        self.handle_map = handle_map
        self.minidrone = minidrone
        self.ble_connection = ble_connection
        color_print("initializing notification delegate", "INFO")

    def handleNotification(self, cHandle, data):
        #print "handling notificiation from channel %d" % cHandle
        #print "handle map is %s " % self.handle_map[cHandle]
        #print "channel map is %s " % self.minidrone.characteristic_receive_uuids[self.handle_map[cHandle]]
        #print "data is %s " % data

        channel = self.ble_connection.characteristic_receive_uuids[self.handle_map[cHandle]]

        (packet_type, packet_seq_num) = struct.unpack('<BB', data[0:2])
        raw_data = data[2:]

        if channel == 'ACK_DRONE_DATA':
            # data received from drone (needs to be ack on 1e)
            #color_print("calling update sensors ack true", "WARN")
            self.minidrone.update_sensors(packet_type, None, packet_seq_num, raw_data, ack=True)
        elif channel == 'NO_ACK_DRONE_DATA':
            # data from drone (including battery and others), no ack
            #color_print("drone data - no ack needed")
            self.minidrone.update_sensors(packet_type, None, packet_seq_num, raw_data, ack=False)
        elif channel == 'ACK_COMMAND_SENT':
            # ack 0b channel, SEND_WITH_ACK
            #color_print("Ack!  command received!")
            self.ble_connection._set_command_received('SEND_WITH_ACK', True)
        elif channel == 'ACK_HIGH_PRIORITY':
            # ack 0c channel, SEND_HIGH_PRIORITY
            #color_print("Ack!  high priority received")
            self.ble_connection._set_command_received('SEND_HIGH_PRIORITY', True)
        else:
            color_print("unknown channel %s sending data " % channel, "ERROR")
            color_print(cHandle)


class BLEConnection:
    def __init__(self, address, minidrone):
        """
             Initialize with its BLE address - if you don't know the address, call findMinidrone
             and that will discover it for you.

             :param address: unique address for this minidrone
             :param minidrone: the Minidrone object for this minidrone (needed for callbacks for sensors)
             """
        self.address = address
        self.drone_connection = Peripheral()
        self.minidrone = minidrone

        # the following UUID segments come from the Minidrone and from the documenation at
        # http://forum.developer.parrot.com/t/minidrone-characteristics-uuid/4686/3
        # the 3rd and 4th bytes are used to identify the service
        self.service_uuids = {
            'fa00': 'ARCOMMAND_SENDING_SERVICE',
            'fb00': 'ARCOMMAND_RECEIVING_SERVICE',
            'fc00': 'PERFORMANCE_COUNTER_SERVICE',
            'fd21': 'NORMAL_BLE_FTP_SERVICE',
            'fd51': 'UPDATE_BLE_FTP',
            'fe00': 'UPDATE_RFCOMM_SERVICE',
            '1800': 'Device Info',
            '1801': 'unknown',
        }
        # the following characteristic UUID segments come from the documentation at
        # http://forum.developer.parrot.com/t/minidrone-characteristics-uuid/4686/3
        # the 4th bytes are used to identify the characteristic
        # the usage of the channels are also documented here
        # http://forum.developer.parrot.com/t/ble-characteristics-of-minidrones/5912/2
        self.characteristic_send_uuids = {
            '0a': 'SEND_NO_ACK',  # not-ack commandsandsensors (PCMD only)
            '0b': 'SEND_WITH_ACK',  # ack commandsandsensors (all piloting commandsandsensors)
            '0c': 'SEND_HIGH_PRIORITY',  # emergency commandsandsensors
            '1e': 'ACK_COMMAND'  # ack for data sent on 0e
        }

        # counters for each packet (required as part of the packet)
        self.characteristic_send_counter = {
            'SEND_NO_ACK': 0,
            'SEND_WITH_ACK': 0,
            'SEND_HIGH_PRIORITY': 0,
            'ACK_COMMAND': 0,
            'RECEIVE_WITH_ACK': 0
        }

        # the following characteristic UUID segments come from the documentation at
        # http://forum.developer.parrot.com/t/minidrone-characteristics-uuid/4686/3
        # the 4th bytes are used to identify the characteristic
        # the types of commandsandsensors and data coming back are also documented here
        # http://forum.developer.parrot.com/t/ble-characteristics-of-minidrones/5912/2
        self.characteristic_receive_uuids = {
            '0e': 'ACK_DRONE_DATA',  # drone data that needs an ack (needs to be ack on 1e)
            '0f': 'NO_ACK_DRONE_DATA',  # data from drone (including battery and others), no ack
            '1b': 'ACK_COMMAND_SENT',  # ack 0b channel, SEND_WITH_ACK
            '1c': 'ACK_HIGH_PRIORITY',  # ack 0c channel, SEND_HIGH_PRIORITY
        }

        # these are the FTP incoming and outcoming channels
        # the handling characteristic seems to be the one to send commandsandsensors to (per the SDK)
        # information gained from reading ARUTILS_BLEFtp.m in the SDK
        self.characteristic_ftp_uuids = {
            '22': 'NORMAL_FTP_TRANSFERRING',
            '23': 'NORMAL_FTP_GETTING',
            '24': 'NORMAL_FTP_HANDLING',
            '52': 'UPDATE_FTP_TRANSFERRING',
            '53': 'UPDATE_FTP_GETTING',
            '54': 'UPDATE_FTP_HANDLING',
        }

        # FTP commandsandsensors (obtained via ARUTILS_BLEFtp.m in the SDK)
        self.ftp_commands = {
            "list": "LIS",
            "get": "GET"
        }

        # need to save for communication (but they are initialized in connect)
        self.services = None
        self.send_characteristics = dict()
        self.receive_characteristics = dict()
        self.handshake_characteristics = dict()
        self.ftp_characteristics = dict()

        self.data_types = {
            'ACK': 1,
            'DATA_NO_ACK': 2,
            'LOW_LATENCY_DATA': 3,
            'DATA_WITH_ACK': 4
        }

        # store whether a command was acked
        self.command_received = {
            'SEND_WITH_ACK': False,
            'SEND_HIGH_PRIORITY': False,
            'ACK_COMMAND': False
        }

        # instead of parsing the XML file every time, cache the results
        self.command_tuple_cache = dict()
        self.sensor_tuple_cache = dict()

        # maximum number of times to try a packet before assuming it failed
        self.max_packet_retries = 3

    def connect(self, num_retries):
        """
        Connects to the drone and re-tries in case of failure the specified number of times

        :param: num_retries is the number of times to retry

        :return: True if it succeeds and False otherwise
        """

        # first try to connect to the wifi
        try_num = 1
        connected = False
        while (try_num < num_retries and not connected):
            try:
                self._connect()
                connected = True
            except BTLEException:
                color_print("retrying connections", "INFO")
                try_num += 1

        # fall through, return False as something failed
        return connected

    def _reconnect(self, num_retries):
        """
        Reconnect to the drone (assumed the BLE crashed)

        :param: num_retries is the number of times to retry

        :return: True if it succeeds and False otherwise
        """
        try_num = 1
        success = False
        while (try_num < num_retries and not success):
            try:
                color_print("trying to re-connect to the minidrone at address %s" % self.address, "WARN")
                self.drone_connection.connect(self.address, "random")
                color_print("connected!  Asking for services and characteristics", "SUCCESS")
                success = True
            except BTLEException:
                color_print("retrying connections", "WARN")
                try_num += 1

        if (success):
            # do the magic handshake
            self._perform_handshake()

        return success

    def _connect(self):
        """
        Connect to the minidrone to prepare for flying - includes getting the services and characteristics
        for communication

        :return: throws an error if the drone connection failed.  Returns void if nothing failed.
        """
        color_print("trying to connect to the minidrone at address %s" % self.address, "INFO")
        self.drone_connection.connect(self.address, "random")
        color_print("connected!  Asking for services and characteristics", "SUCCESS")

        # re-try until all services have been found
        allServicesFound = False

        # used for notifications
        handle_map = dict()

        while not allServicesFound:
            # get the services
            self.services = self.drone_connection.getServices()

            # loop through the services
            for s in self.services:
                hex_str = self._get_byte_str_from_uuid(s.uuid, 3, 4)

                # store the characteristics for receive & send
                if (self.service_uuids[hex_str] == 'ARCOMMAND_RECEIVING_SERVICE'):
                    # only store the ones used to receive data
                    for c in s.getCharacteristics():
                        hex_str = self._get_byte_str_from_uuid(c.uuid, 4, 4)
                        if hex_str in self.characteristic_receive_uuids:
                            self.receive_characteristics[self.characteristic_receive_uuids[hex_str]] = c
                            handle_map[c.getHandle()] = hex_str


                elif (self.service_uuids[hex_str] == 'ARCOMMAND_SENDING_SERVICE'):
                    # only store the ones used to send data
                    for c in s.getCharacteristics():
                        hex_str = self._get_byte_str_from_uuid(c.uuid, 4, 4)
                        if hex_str in self.characteristic_send_uuids:
                            self.send_characteristics[self.characteristic_send_uuids[hex_str]] = c


                elif (self.service_uuids[hex_str] == 'UPDATE_BLE_FTP'):
                    # store the FTP info
                    for c in s.getCharacteristics():
                        hex_str = self._get_byte_str_from_uuid(c.uuid, 4, 4)
                        if hex_str in self.characteristic_ftp_uuids:
                            self.ftp_characteristics[self.characteristic_ftp_uuids[hex_str]] = c

                elif (self.service_uuids[hex_str] == 'NORMAL_BLE_FTP_SERVICE'):
                    # store the FTP info
                    for c in s.getCharacteristics():
                        hex_str = self._get_byte_str_from_uuid(c.uuid, 4, 4)
                        if hex_str in self.characteristic_ftp_uuids:
                            self.ftp_characteristics[self.characteristic_ftp_uuids[hex_str]] = c

                # need to register for notifications and write 0100 to the right handles
                # this is sort of magic (not in the docs!) but it shows up on the forum here
                # http://forum.developer.parrot.com/t/minimal-ble-commands-to-send-for-take-off/1686/2
                # Note this code snippet below more or less came from the python example posted to that forum (I adapted it to my interface)
                for c in s.getCharacteristics():
                    if self._get_byte_str_from_uuid(c.uuid, 3, 4) in \
                            ['fb0f', 'fb0e', 'fb1b', 'fb1c', 'fd22', 'fd23', 'fd24', 'fd52', 'fd53', 'fd54']:
                        self.handshake_characteristics[self._get_byte_str_from_uuid(c.uuid, 3, 4)] = c

            # check to see if all 8 characteristics were found
            allServicesFound = True
            for r_id in self.characteristic_receive_uuids.values():
                if r_id not in self.receive_characteristics:
                    color_print("setting to false in receive on %s" % r_id)
                    allServicesFound = False

            for s_id in self.characteristic_send_uuids.values():
                if s_id not in self.send_characteristics:
                    color_print("setting to false in send")
                    allServicesFound = False

            for f_id in self.characteristic_ftp_uuids.values():
                if f_id not in self.ftp_characteristics:
                    color_print("setting to false in ftp")
                    allServicesFound = False

            # and ensure all handshake characteristics were found
            if len(self.handshake_characteristics.keys()) != 10:
                color_print("setting to false in len")
                allServicesFound = False

        # do the magic handshake
        self._perform_handshake()

        # initialize the delegate to handle notifications
        self.drone_connection.setDelegate(MinidroneDelegate(handle_map, self.minidrone, self))

    def _perform_handshake(self):
        """
        Magic handshake
        Need to register for notifications and write 0100 to the right handles
        This is sort of magic (not in the docs!) but it shows up on the forum here
        http://forum.developer.parrot.com/t/minimal-ble-commandsandsensors-to-send-for-take-off/1686/2

        :return: nothing
        """
        color_print("magic handshake to make the drone listen to our commandsandsensors")

        # Note this code snippet below more or less came from the python example posted to that forum (I adapted it to my interface)
        for c in self.handshake_characteristics.values():
            # for some reason bluepy characteristic handle is two lower than what I need...
            # Need to write 0x0100 to the characteristics value handle (which is 2 higher)
            self.drone_connection.writeCharacteristic(c.handle + 2, struct.pack("<BB", 1, 0))

    def disconnect(self):
        """
        Disconnect the BLE connection.  Always call this at the end of your programs to
        cleanly disconnect.

        :return: void
        """
        self.drone_connection.disconnect()

    def _get_byte_str_from_uuid(self, uuid, byte_start, byte_end):
        """
        Extract the specified byte string from the UUID btle object.  This is an ugly hack
        but it was necessary because of the way the UUID object is represented and the documentation
        on the byte strings from Parrot.  You give it the starting byte (counting from 1 since
        that is how their docs count) and the ending byte and it returns that as a string extracted
        from the UUID.  It is assumed it happens before the first - in the UUID.

        :param uuid: btle UUID object
        :param byte_start: starting byte (counting from 1)
        :param byte_end: ending byte (counting from 1)
        :return: string with the requested bytes (to be used as a key in the lookup tables for services)
        """
        uuid_str = format("%s" % uuid)
        idx_start = 2 * (byte_start - 1)
        idx_end = 2 * (byte_end)

        my_hex_str = uuid_str[idx_start:idx_end]
        return my_hex_str


    def send_turn_command(self, command_tuple, degrees):
        """
        Build the packet for turning and send it

        :param command_tuple: command tuple from the parser
        :param degrees: how many degrees to turn
        :return: True if the command was sent and False otherwise
        """
        self.characteristic_send_counter['SEND_WITH_ACK'] = (self.characteristic_send_counter['SEND_WITH_ACK'] + 1) % 256

        packet = struct.pack("<BBBBHh", self.data_types['DATA_WITH_ACK'],
                             self.characteristic_send_counter['SEND_WITH_ACK'],
                             command_tuple[0], command_tuple[1], command_tuple[2],
                             degrees)

        return self.send_command_packet_ack(packet)

    def send_auto_takeoff_command(self, command_tuple):
        """
        Build the packet for auto takeoff and send it

        :param command_tuple: command tuple from the parser
        :return: True if the command was sent and False otherwise
        """
        # print command_tuple
        self.characteristic_send_counter['SEND_WITH_ACK'] = (
                                                                self.characteristic_send_counter[
                                                                    'SEND_WITH_ACK'] + 1) % 256
        packet = struct.pack("<BBBBHB", self.data_types['DATA_WITH_ACK'],
                             self.characteristic_send_counter['SEND_WITH_ACK'],
                             command_tuple[0], command_tuple[1], command_tuple[2],
                             1)

        return self.send_command_packet_ack(packet)


    def send_command_packet_ack(self, packet):
        """
        Sends the actual packet on the ack channel.  Internal function only.

        :param packet: packet constructed according to the command rules (variable size, constructed elsewhere)
        :return: True if the command was sent and False otherwise
        """
        try_num = 0
        self._set_command_received('SEND_WITH_ACK', False)
        while (try_num < self.max_packet_retries and not self.command_received['SEND_WITH_ACK']):
            color_print("sending command packet on try %d" % try_num, 2)
            self._safe_ble_write(characteristic=self.send_characteristics['SEND_WITH_ACK'], packet=packet)
            #self.send_characteristics['SEND_WITH_ACK'].write(packet)
            try_num += 1
            color_print("sleeping for a notification", 2)
            #notify = self.drone.waitForNotifications(1.0)
            self.smart_sleep(0.5)
            #color_print("awake %s " % notify, 2)

        return self.command_received['SEND_WITH_ACK']

    def send_single_pcmd_command(self, command_tuple, roll, pitch, yaw, vertical_movement):
        """
        Send a single PCMD command with the specified roll, pitch, and yaw.  Note
        this will not make that command run forever.  Instead it sends ONCE.  This can be used
        in a loop (in your agent) that makes more smooth control than using the duration option.

        :param command_tuple: command tuple per the parser
        :param roll:
        :param pitch:
        :param yaw:
        :param vertical_movement:
        """

        self.characteristic_send_counter['SEND_NO_ACK'] = (self.characteristic_send_counter['SEND_NO_ACK'] + 1) % 256
        packet = struct.pack("<BBBBHBbbbbI", self.data_types['DATA_NO_ACK'],
                             self.characteristic_send_counter['SEND_NO_ACK'],
                             command_tuple[0], command_tuple[1], command_tuple[2],
                             1, int(roll), int(pitch), int(yaw), int(vertical_movement), 0)

        self._safe_ble_write(characteristic=self.send_characteristics['SEND_NO_ACK'], packet=packet)
        # self.send_characteristics['SEND_NO_ACK'].write(packet)

    def send_pcmd_command(self, command_tuple, roll, pitch, yaw, vertical_movement, duration):
        """
        Send the PCMD command with the specified roll, pitch, and yaw

        :param command_tuple: command tuple per the parser
        :param roll:
        :param pitch:
        :param yaw:
        :param vertical_movement:
        :param duration:
        """
        start_time = time.time()
        while (time.time() - start_time < duration):

            self.send_single_pcmd_command(command_tuple, roll, pitch, yaw, vertical_movement)
            notify = self.drone_connection.waitForNotifications(0.1)

    def send_noparam_command_packet_ack(self, command_tuple):
        """
        Send a command on the ack channel - where all commandsandsensors except PCMD go, per
        http://forum.developer.parrot.com/t/ble-characteristics-of-minidrones/5912/2

        the id of the last command sent (for use in ack) is the send counter (which is incremented before sending)

        Ensures the packet was received or sends it again up to a maximum number of times.

        :param command_tuple: 3 tuple of the command bytes.  0 padded for 4th byte
        :return: True if the command was sent and False otherwise
        """
        self.characteristic_send_counter['SEND_WITH_ACK'] = (self.characteristic_send_counter['SEND_WITH_ACK'] + 1) % 256
        packet = struct.pack("<BBBBH", self.data_types['DATA_WITH_ACK'], self.characteristic_send_counter['SEND_WITH_ACK'],
                             command_tuple[0], command_tuple[1], command_tuple[2])
        return self.send_command_packet_ack(packet)



    def send_enum_command_packet_ack(self, command_tuple, enum_value, usb_id=None):
        """
        Send a command on the ack channel with enum parameters as well (most likely a flip).
        All commandsandsensors except PCMD go on the ack channel per
        http://forum.developer.parrot.com/t/ble-characteristics-of-minidrones/5912/2

        the id of the last command sent (for use in ack) is the send counter (which is incremented before sending)

        :param command_tuple: 3 tuple of the command bytes.  0 padded for 4th byte
        :param enum_value: the enum index
        :return: nothing
        """
        self.characteristic_send_counter['SEND_WITH_ACK'] = (self.characteristic_send_counter['SEND_WITH_ACK'] + 1) % 256
        if (usb_id is None):
            packet = struct.pack("<BBBBBBI", self.data_types['DATA_WITH_ACK'], self.characteristic_send_counter['SEND_WITH_ACK'],
                                 command_tuple[0], command_tuple[1], command_tuple[2], 0,
                                 enum_value)
        else:
            color_print((self.data_types['DATA_WITH_ACK'], self.characteristic_send_counter['SEND_WITH_ACK'],
                         command_tuple[0], command_tuple[1], command_tuple[2], 0, usb_id, enum_value), 1)
            packet = struct.pack("<BBBBHBI", self.data_types['DATA_WITH_ACK'], self.characteristic_send_counter['SEND_WITH_ACK'],
                                 command_tuple[0], command_tuple[1], command_tuple[2],
                                 usb_id, enum_value)
        return self.send_command_packet_ack(packet)

    def send_param_command_packet(self, command_tuple, param_tuple=None, param_type_tuple=0, ack=True):
        """
        Send a command packet with parameters. Ack channel is optional for future flexibility,
        but currently commands are always send over the Ack channel so it defaults to True.

        Contributed by awm102 on github.  Edited by Amy McGovern to work for BLE commands also.

        :param: command_tuple: the command tuple derived from command_parser.get_command_tuple()
        :param: param_tuple (optional): the parameter values to be sent (can be found in the XML files)
        :param: param_size_tuple (optional): a tuple of strings representing the data type of the parameters
        e.g. u8, float etc. (can be found in the XML files)
        :param: ack (optional): allows ack to be turned off if required
        :return:
        """
        # Create lists to store the number of bytes and pack chars needed for parameters
        # Default them to zero so that if no params are provided the packet size is correct
        param_size_list = [0] * len(param_tuple)
        pack_char_list = [0] * len(param_tuple)

        if param_tuple is not None:
            # Fetch the parameter sizes. By looping over the param_tuple we only get the data
            # for requested parameters so a mismatch in params and types does not matter
            for i, param in enumerate(param_tuple):
                pack_char_list[i], param_size_list[i] = get_data_format_and_size(param, param_type_tuple[i])

        if ack:
            ack_string = 'SEND_WITH_ACK'
            data_ack_string = 'DATA_WITH_ACK'
        else:
            ack_string = 'SEND_NO_ACK'
            data_ack_string = 'DATA_NO_ACK'

        # Construct the base packet
        self.characteristic_send_counter['SEND_WITH_ACK'] = (self.characteristic_send_counter['SEND_WITH_ACK'] + 1) % 256

        # TODO:  Amy changed this to match the BLE packet structure but needs to fully test it
        packet = struct.pack("<BBBBH", self.data_types[data_ack_string],
                             self.characteristic_send_counter[ack_string],
                             command_tuple[0], command_tuple[1], command_tuple[2])

        if param_tuple is not None:
            # Add in the parameter values based on their sizes
            for i, param in enumerate(param_tuple):
                packet += struct.pack(pack_char_list[i], param)

        # TODO: Fix this to not go with ack always
        return self.send_command_packet_ack(packet)

    def _set_command_received(self, channel, val):
        """
        Set the command received on the specified channel to the specified value (used for acks)

        :param channel: channel
        :param val: True or False
        :return:
        """
        self.command_received[channel] = val

    def _safe_ble_write(self, characteristic, packet):
        """
        Write to the specified BLE characteristic but first ensure the connection is valid

        :param characteristic:
        :param packet:
        :return:
        """

        success = False

        while (not success):
            try:
                characteristic.write(packet)
                success = True
            except BTLEException:
                color_print("reconnecting to send packet", "WARN")
                self._reconnect(3)

    def ack_packet(self, buffer_id, packet_id):
        """
        Ack the packet id specified by the argument on the ACK_COMMAND channel

        :param packet_id: the packet id to ack
        :return: nothing
        """
        #color_print("ack last packet on the ACK_COMMAND channel", "INFO")
        self.characteristic_send_counter['ACK_COMMAND'] = (self.characteristic_send_counter['ACK_COMMAND'] + 1) % 256
        packet = struct.pack("<BBB", self.data_types['ACK'], self.characteristic_send_counter['ACK_COMMAND'],
                             packet_id)
        #color_print("sending packet %d %d %d" % (self.data_types['ACK'], self.characteristic_send_counter['ACK_COMMAND'],
        #                                   packet_id), "INFO")

        self._safe_ble_write(characteristic=self.send_characteristics['ACK_COMMAND'], packet=packet)
        #self.send_characteristics['ACK_COMMAND'].write(packet)


    def smart_sleep(self, timeout):
        """
        Sleeps the requested number of seconds but wakes up for notifications

        Note: NEVER use regular time.sleep!  It is a blocking sleep and it will likely
        cause the BLE to disconnect due to dropped notifications.  Always use smart_sleep instead!

        :param timeout: number of seconds to sleep
        :return:
        """

        start_time = datetime.now()
        new_time = datetime.now()
        diff = (new_time - start_time).seconds + ((new_time - start_time).microseconds / 1000000.0)

        while (diff < timeout):
            try:
                notify = self.drone_connection.waitForNotifications(0.1)
            except:
                color_print("reconnecting to wait", "WARN")
                self._reconnect(3)

            new_time = datetime.now()
            diff = (new_time - start_time).seconds + ((new_time - start_time).microseconds / 1000000.0)
