import can
import struct
import time
import argparse

# Константы
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x00          # ID ADDR for servo
REG_WRITE = 0x8
REG_POS = 0x72             # MOTOR+ANGLE = 0x72

def validate_crc16(data):
    #   Calculate   CRC16
    crc = 0xFFFF
    for byte in data:
        crc ^= byte
        for _ in range(8):
            if crc & 0x0001:
                crc = (crc >> 1) ^ 0xA001
            else:
                crc >>= 1
    return crc

def send_target_angle(bus, target_angle):
    # ID and cmd
    arbitration_id = (DEVICE_ID << 4) | REG_WRITE
    id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))

    # cmd + parametrs
    data_write = [REG_POS] + list(struct.pack('<f', target_angle))  

    
    full_data_for_crc = id_bytes + data_write
    crc = validate_crc16(full_data_for_crc)
    crc_bytes = list(crc.to_bytes(2, byteorder='little'))

    # Full packet
    packet = data_write + crc_bytes


    msg = can.Message(
        arbitration_id=arbitration_id,
        is_extended_id=False,
        data=packet
    )

    try:
        bus.send(msg)
        print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
    except can.CanError:
        print("Ошибка отправки сообщения")

def main():
    parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
    parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
    args = parser.parse_args()

    # Инициализация CAN
    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
    print("CAN шина инициализирована.")

    send_target_angle(bus, args.angle)

    bus.shutdown()
if __name__ == '__main__':
    main()