# AUTOGENERATED! DO NOT EDIT! File to edit: ../10_xbee_coding.ipynb.

# %% auto 0
__all__ = ['np_array_to_Odometry', 'xbee_encode', 'xbee_decode']

# %% ../10_xbee_coding.ipynb 4
import pickle

def np_array_to_Odometry(array):
    msg = Odometry()
    msg.header.frame_id = "odom"
    msg.pose.pose.position.x = array[0]
    msg.pose.pose.position.y = array[1]
    msg.pose.pose.position.z = array[2]
    msg.pose.pose.orientation.x = array[3]
    msg.pose.pose.orientation.y = array[4]
    msg.pose.pose.orientation.z = array[5]
    msg.pose.pose.orientation.w = array[6]
    msg.twist.twist.linear.x = array[7]
    msg.twist.twist.linear.y = array[8]
    msg.twist.twist.linear.z = array[9]
    msg.twist.twist.angular.x = array[10]
    msg.twist.twist.angular.y = array[11]
    msg.twist.twist.angular.z = array[12]
    return msg

def xbee_encode(data_via_xbee, data_type):
    data = data_via_xbee

    # send data
    byte_arr = pickle.dumps( data )
    length, index, check= int(len(byte_arr)), 0, 0

    for index in range(0,length,250) :
        pack = bytearray(b'\xAB') #Header
        pack.extend(bytearray(data_type)) #Type
        pack.extend( length.to_bytes(4, byteorder='big') ) #bytes
        index_end = index+250 if index+250 < length else length
        pack.extend( byte_arr[index:(index_end)] ) #data

        if index_end == length : pack.extend(check.to_bytes(1, byteorder='big')) # checksum
        else: check = 0xff & (check + pack[-1])

    return pack  

def xbee_decode(xbee_message):
    print(xbee_message)

    if not xbee_message[0:1] == b'\xAB' : # Header wrong
        print('get xbee_message with wrong Header')
        return

    if not ((xbee_message[1:2] == b'\x00') or (xbee_message[1:2] == b'\x01') or (xbee_message[1:2] == b'\x02') or (xbee_message[1:2] == b'\x03')):
        rospy.loginfo('xbeejoy callback')
        self.count += 1
        get_msg = pickle.loads(xbee_message)
        axes = get_msg[0:8]
        buttons = get_msg[8:]
        rospy.loginfo(axes)
        rospy.loginfo(buttons)

        msg = Joy()
        msg.header.seq = self.count
        msg.header.frame_id = "/dev/input/js0"
        msg.header.stamp = rospy.Time.now()
        msg.axes = axes
        msg.buttons = buttons
        return msg
    
    get_register.extend(xbee_message[6:])

    if xbee_message[1:2] == b'\x00':
        get_msg = pickle.loads(get_register[:-1])
        print(get_msg)
        return(get_msg)
    
    
    if xbee_message[1:2] == b'\x02': # type: points
        get_points = pickle.loads(get_register[:-1])
        pub_msg = np_array_to_Odometry(get_points)
        return(pub_msg)
