import serial
import time

# Configuração da porta serial
ser = serial.Serial(
    port='COM19',  # Substitua pelo seu porto serial
    baudrate=38400,
    timeout=1
)

slave_address = 0x01  # Endereço do escravo

Parametros = {
    "EncoderCarry": 0x30,
    "EncoderAddition": 0x31,
    "RPM": 0x32,
    "PulsesReceived": 0x33,
    "ErrorShaftAngle": 0x39,
    "EnPinStatus": 0x3A,
    "GoBackZeroStatus": 0x3B,
    "ProtectionRotorLockedStatus": 0x3D,
    "ProtectionShaftStatus": 0x3E,
    "Calibrate": 0x80,
    "Mode": 0x82,
    "MaxCurrent": 0x83,
    "Subdivision": 0x84,
    "EnPin": 0x85,
    "Direction": 0x86,
    "MotorProtection": 0x88,
    "SubdivisionInterpolation": 0x89,
    "BaudRate": 0x8A,
    "SlaveAddress": 0x8B,
    "SlaveRespond": 0x8C,
    "GoHomeParameters": 0x90,
    "GoHome": 0x91,
    "Default": 0x3F,
}


def crc16(data: bytes):
    crc = 0xFFFF
    for pos in data:
        crc ^= pos
        for i in range(8):
            if (crc & 1) != 0:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    return crc.to_bytes(2, byteorder='little')

def comando_get(parametro):
    function_code = 0xFA
    register = Parametros[parametro]
    command = bytearray([function_code, slave_address, register])
    command += crc16(command)
    return command

def enviar_comando(comando):
    ser.write(comando)
    time.sleep(0.1)  # Pequena pausa para garantir o envio
    resposta = ser.read(ser.inWaiting())  # Lê todos os dados disponíveis na porta
    return resposta

def decifrar_resposta(resposta):
    # Certifique-se de que a resposta é do tipo bytes
    if isinstance(resposta, str):
        resposta = bytes.fromhex(resposta)

    resHex = resposta.hex()
    
    print(resposta)
    print(resposta.hex())

    # Verifica o comprimento da resposta e o código da função
    if len(resposta) >= 5 and resposta[0] == 0xFB:
        remetente = resposta[1]
        # Extrai o número de bytes de dados
        num_bytes = 2

        # Verifica se o comprimento da resposta é consistente com o número de bytes de dados
        if len(resposta) == 5 + num_bytes:
            valor_raw = int.from_bytes(resposta[3:3 + num_bytes], byteorder='big')
            
            valor = valor_raw # * 1  # Ajuste o fator de multiplicação conforme necessário

            print(f"Remetente: {remetente}, Valor: {valor}")
            return (remetente, valor)
        else:
            print("Comprimento da resposta inconsistente com o número de bytes de dados.")
    else:
        print("Resposta inválida ou código de função incorreto.")
    return (0, None)


try:
    #res = enviar_comando(comando_get("EncoderCarry"))
    com = [0xFA, 0x01, 0x30, 0x2B]
    com1 = [0xFA, 0x01, 0xFF, 0xC8]
    res = enviar_comando(com1)
    time.sleep(1)
    decifrar_resposta(res)
except KeyboardInterrupt:
    print("Programa encerrado pelo usuário")
finally:
    ser.close()