import socket
import threading
import time

class Tello(object):
    """
    Wrapper class to interact with the Tello drone.
    """
    def __init__(self, local_ip, local_port, imperial=False, 
                 command_timeout = 0.3, 
                 tello_ip = '192.168.10.1',
                 tello_port = 8889):
        """
        Binds to the local IP/port and puts the Tello into command mode.

        :param local_ip: Local IP address to bind.
        :param local_port: Local port to bind.
        :param imperial: If True, speed is MPH and distance is feet. 
                         If False, speed is KPH and distance is meters.
        :param command_timeout: Number of seconds to wait for a response to a command.
        :param tello_ip: Tello IP.
        :param tello_port: Tello port.
        """
        self.abort_flag = False
        self.command_timeout = command_timeout
        self.imperial = imperial
        self.response = None
        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.tello_address = (tello_ip, tello_port)
        self.last_height = 0
        self.socket.bind((local_ip, local_port))

        # thread for receiving cmd ack
        self.receive_thread = threading.Thread(target = self._receive_thread)
        self.receive_thread.daemon = True
        self.receive_thread.start()

        self.socket.sendto(b'command', self.tello_address)
        print('sent: command')

    def __del__(self):
        """
        Closes the local socket.

        :return: None.
        """
        self.socket.close()

    def _receive_thread(self):
        """
        Listen to responses from the Tello.

        Runs as a thread, sets self.response to whatever the Tello last returned.

        :return: None.
        """
        while True:
            try:
                self.response, _ = self.socket.recvfrom(3000)
            except socket.error as exc:
                print(f'Caught exception socket.error : {exc}')

    def send_command(self, command):
        """
        Send a command to the Tello and wait for a response.

        :param command: Command to send.
        :return: Response from Tello.
        """
        print(f'>> send cmd: {command}')
        self.abort_flag = False
        timer = threading.Timer(self.command_timeout, self.set_abort_flag)

        self.socket.sendto(command.encode('utf-8'), self.tello_address)

        timer.start()
        while self.response is None:
            if self.abort_flag is True:
                break
        timer.cancel()
        
        if self.response is None:
            response = 'none_response'
        else:
            response = self.response.decode('utf-8')

        self.response = None

        return response
    
    def set_abort_flag(self):
        """
        Sets self.abort_flag to True.

        Used by the timer in Tello.send_command() to indicate to that a response        
        timeout has occurred.

        :return: None.
        """
        self.abort_flag = True

    def takeoff(self):
        """
        Initiates take-off.

        :return: Response from Tello, 'OK' or 'FALSE'.
        """
        return self.send_command('takeoff')
    
    def land(self):
        """
        Initiates landing.

        :return: Response from Tello, 'OK' or 'FALSE'.
        """
        return self.send_command('land')