"""
crazyflie_connection handles the actual interaction with the crazyflie.
The only reason it exists is to serve as an intermediary between the groundstation
and the crazyflie itself so that it can handle all interactions with the
cflib library.
"""

from time import time
from typing import List
import time
import struct

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from queue import Queue
#import groundstation_socket as gs
import uCartCommander
from groundstation_socket import MessageTypeID
from SetpointHandler import SetpointHandler, FlightMode
from cflib.crazyflie.log import LogConfig


class CrazyflieConnection:

    """
    Handles all interactions with cflib.
    """

    def __init__(self):
        """
        Initialize the start time, the logging queue which is given to the
        plotting window, and set the synchronous crazyflie connection by
        default to None. This should be set to the crazyflie when connecting
        to one.
        """

        # Get the start time, so that the timestamp returned to the user will
        # be in seconds since startup.
        self.start_time = time.time()

        self.logging_queue = Queue()

        self.scf = None
        self.is_connected = False
        self.param_callback_count = 0
        self.logging_configs = []

        self.setpoint_handler = SetpointHandler()

       # self.setpoint_handler.setCommander(self.scf.cf.commander)

        # self.timer = QTimer()
        # self.timer.timeout.connect(self.update_plot)
        # self.timer.start(50)

    def connect(self, uri: str):
        """
        Handles connecting to a crazyflie. Bitcraze has excellent
        documentation on how to use the synchronous crazyflie object in order
        to send setpoints, set parameters or retrieve logging.
        :param uri: Radio channel
        """
        cflib.crtp.init_drivers()
        self.scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
        self.scf.open_link()
        self.scf.wait_for_params()
        self.is_connected = True

        # sets commander
        self.scf.cf.commander = uCartCommander.Commander(self.scf.cf)


        # connect the crazyflie commander to the setpoint handler
        # refresh the logging page so that it displays the toc
        # refresh the parameter page so that it displays the correct information
        self.setpoint_handler.setCommander(self.scf.cf.commander)

    def disconnect(self):
        """ Disconnect from crazyflie. """
        print("Disconnect quad")
        if self.is_connected:
            self.scf.close_link()
            self.scf = None
            self.is_connected = False

    def Debug():
        raise Exception
    def PacketLog():
        raise Exception
    def GetPacketLogs():
        raise Exception
    def Update():
        raise Exception
    def BeginUpdate():
        raise Exception
    def OverrideOuput(self, command): 
        """Sends all setpoints for a given amount of time"""
        print(command["data"])
        mode = command['data'][0]
        time = command['data'][1:5] # Currently sent every 20ms by setpoint_handler may change
        thrust =  command['data'][5:9]
        pitch =  command['data'][9:13]
        roll =  command['data'][13:17]
        yaw =  command['data'][17:21]

        # Error Handling
        try:
            yaw = struct.unpack('f', bytes(yaw))[0]
            pitch = struct.unpack('f', bytes(pitch))[0]
            roll = struct.unpack('f', bytes(roll))[0]
            thrust = struct.unpack('f', bytes(thrust))[0]
            print(thrust)
        except ValueError:
            raise Exception

        # Check that setpoint_handler has a commander set
        if self.setpoint_handler.commander:
            # Lets crazyflie know we are about to start flying
            self.setpoint_handler.commander.start_flying()

            if mode == 0: # Stop?
                self.setpoint_handler.stopFlying()
            elif mode == 1: # Rate
                self.setpoint_handler.setRateMode()
            elif mode == 2: # Angle
                self.setpoint_handler.setAttitudeMode()
            elif mode == 3: # Mixed
                self.setpoint_handler.setMixedAttitudeMode()
            elif mode == 4: # Position
                raise Exception # Not implemented
            else :
                raise Exception

            self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)

    def GetNodeIds():
        raise Exception
    def SetParam(self, command):
        """ Set a crazyflie parameter value. """
        group = command['data'][0:1]
        name = command['data'][2:3]
        value =  command['data'][4:7]
        try:
            if self.scf.is_link_open():
                full_name = group + "." + name
                cf = self.scf.cf
                cf.param.add_update_callback(group=group, name=name,
                                             cb=self.done_setting_param_value)

                cf.param.set_value(full_name, value)

                # Don't return until the parameter is done getting set
                while self.param_callback_count < 1:
                    time.sleep(0.01)

                self.param_callback_count = 0
        except AttributeError:
            print("Nothing connected")

    def done_setting_param_value(self, *_args):
        """ Callback when done setting a parameter value. """
        print("Done setting param")
        self.param_callback_count += 1

    def GetParam(self, command, outputQueue: Queue): #group: str, name: str,
        """ Retrieve parameter value from crazyflie toc. """
        #Bytes 0 and 1 are node ID, ie group
        #Bytes 2 and 3 are node paramID, ie name
        print("Getting Param...")
        group = command['data'][0:1]
        name = command['data'][2:3]


        try:
            if self.scf.is_link_open():
                val = self.scf.cf.param.values[group][name]
                print(val)
    

        except AttributeError:
            val = -1.234567
            pass

            
        data = bytearray(group)
        data.append(name)
        data.append(val)
        print(data)
        responsedata = {
        "msg_type": (MessageTypeID.RESPPARAM_ID),
        "msg_id": command['msg_id'],
        "data_len": 8,
        "data": data
        }
        outputQueue.put(responsedata)

        #return -1.234567890  # 1234567890 should be pretty obvious that
        # something has gone wrong.
    def SetSource():
        raise Exception
    def GetSource():
        raise Exception
    def RespSource():
        raise Exception
    def GetOutput():
        raise Exception
    def GetNodes():
        raise Exception
    def AddNode():
        raise Exception
    def GetLogFile(): 
        #TODO
        raise Exception
    def LogBlockCommand(): 
        #TODO
        raise Exception