import utime
'''
发送数据格式：
        角速度线速度(元组), imu数据(元组)
          (现有数据)          (现有数据)
    示例：
        ((angular_vel,linear_vel),((AcX,AcY,AcZ),(GyX,GyY,GyZ),(MgX, MgY, MgZ))
接收数据格式：
        角速度线速度(元组)
    示例:
        (angular_vel, linear_vel)
'''

class Serial:
    def __init__(self, uart):
        self.uart = uart

    def serial_check(self, ser_data):
        '''
        函数功能：检查串口是否接受到数据
        '''
        if ser_data != None:
            return True
        else:
            return False

    def send_data(self, data):
        '''
        函数功能：串口发送数据
        入口参数：需要发送的打包好的数据（字符串格式）
        '''
        self.uart.write(data) # 使用UART2时的发送函数
        # self.uart.send(data, timeout=5) # 使用USB_VCP时的发送函数

        # print(data)

    def format_data(self,data_1=(0,0), data_2=(0,0,0)):
        '''
        函数功能：将需要发送的所有数据封装起来
        入口参数：所有需要发送的数据
        返回值  ：封装后的数据元组
        '''
        data = (data_1, data_2)
        zip_data = (str(data) + "\n").encode("utf-8") #  
        # print(zip_data)
        return zip_data
    
    def rev_data(self):
        '''
        函数功能：读取上位机发送的运动控制指令
        入口参数：无
        返回值  ：接收到的数据（已经被转化为可读取的元组形式）
        '''
        start = utime.ticks_ms()
        data = self.uart.readline()
        if data != None:
            try:
                data = self.analysis_data(data)
                return data
            except:
                pass
            # print(data)
        end = utime.ticks_ms()
        # print("serial running time:", end - start)
        
    def analysis_data(self, data):
        '''
        函数功能：解析串口数据并返回解析完成的元组数据
        入口参数：串口直接接受的数据
        返回值  ：解析后的元组数据
        '''
        data.decode("utf-8")
        data = str(data)[2:-3] # 将 “\n” 和 b'' 去除
        data = eval(data) # 转化回元组形式
        # print(type(data))
        return data

    def serial_pc(self, target_enc_A, enc_A, target_enc_B, enc_B ):
        '''
        函数功能：将需要发送到PC上位机（匿名工作站）的PID或其他参数转换成固定协议的格式，该函数需配合“匿名地面工作站”一起使用，用于PID实时绘图调参
        入口参数：需要发送的数据
        返回值  ：无
                            数据头  功能帧  数据长度   数据（任意多组）         校验位
        @ 匿名工作站通信协议： AAAA    F1     xx      xxxx xxxx xxxx xxxx      sum(前边所有数据之和的低8位)      
        '''
        def data_params_process(*params):
            '''
            @ 数据结构主体，传入任意需要发送的参数
            '''
            # 主数据
            int_data_list = []
            hex_data_list = []
            for data in params:
                int_data = int(data)
                if int_data >= 0: # 如果为正
                    hex_data = hex(int_data)[2:] # 去除0x
                    # 数据位处理
                    if len(hex_data) <= 1:
                        hex_data = "000" + hex_data
                    elif len(hex_data) <= 2:
                        hex_data = "00" + hex_data
                    elif len(hex_data) <= 3:
                        hex_data = "0" + hex_data
                else: # 负数
                    int_data = int_data + 2**16 # 负数需将其加2^16，使其为 FFFF - abs(int_data)
                    hex_data = hex(int_data)[2:] # 去除0x
                int_data_list.append(int_data)
                hex_data_list.append(hex_data)
            return int_data_list, hex_data_list
        
        # 数据位
        dec_data_list, hex_data_list = data_params_process(target_enc_A, enc_A, target_enc_B, enc_B ) # 输入数据

        # 数据头
        head1 = "AA"
        head2 = "AA"
        head3 = "F1"
        
        # 长度位
        length = str(2 * len(hex_data_list)) 
        if len(length) <= 1:
            length = "0" + length

        # 校验位
        sum_head = int(head1, 16) + int(head2, 16) + int(head3, 16) + int(length, 16) # 数据头校验位
        sum_data = 0
        sum_data_list = []
        for i,data in enumerate(dec_data_list): # 数据体校验位
            sum_data = int(data/256) + int(data%256) + sum_data
            sum_data_list.append(int(data/256))
            sum_data_list.append(int(data%256))
        check_sum = (sum_head + sum_data) % 256
        
        # 整理发送数据
        send_data_list = [int(head1, 16), int(head2, 16), int(head3, 16), int(length, 16)] + sum_data_list + [check_sum]
        
        # 发送数据
        for i, send_data in enumerate(send_data_list):
            self.uart.writechar(send_data)