'''
Author: liu xiaoyan
Date: 2023-12-01 14:32:04
LastEditors: WZX 17839623189@168.com
LastEditTime: 2023-12-05 14:02:25
FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/mqtt_msg_decode.py
'''
import binascii
import struct
import math

def calcu_distance(point_a, point_b):
    return math.sqrt(math.pow(point_a[0]-point_b[0], 2)+math.pow(point_a[1]-point_b[1], 2))

def calcu_vecloty(a, b):
    return math.sqrt(math.pow(a, 2) + math.pow(b, 2))

def parse_robot_state(payload):
    msg_payload = binascii.hexlify(payload)
    data_bytes = bytes.fromhex(msg_payload.decode())
    rpy = [struct.unpack_from('<h', data_bytes, offset=2*t)[0] for t in range(3)]
    motorTotations = [struct.unpack_from('<h', data_bytes, offset=2*t+6)[0] for t in range(12)]
    position = [round(struct.unpack_from('<f', data_bytes, offset=4*t+52)[0], 3) for t in range(3)]
    bodyHeight = round(struct.unpack_from('<f', data_bytes, offset=64)[0], 4)
    velocity = [round(struct.unpack_from('<f', data_bytes, offset=4*t+68)[0], 3) for t in range(3)]
    return{
        "rpy": rpy,
        "forward_left": motorTotations[3:6],
        "forward_right": motorTotations[0:3],
        "back_left": motorTotations[9:],
        "back_right": motorTotations[6:9],
        "position": position,
        "bodyHeight": bodyHeight,
        "velocity": velocity
    }