import can
import time
import struct
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = int(sys.argv[1])          # ID ADDR for servo
REG_WRITE = 0x8         # Код команды записи
REG_MOTOR_POSPID_Kp = 0x30
REG_MOTOR_POSPID_Ki = 0x31
REG_MOTOR_POSPID_Kd = 0x32

def send_can_message(bus, can_id, data):
    """Отправка CAN-сообщения"""
    try:
        msg = can.Message(
            arbitration_id=can_id,
            data=data,
            is_extended_id=False
        )
        bus.send(msg)
        print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
        return True
    except can.CanError as e:
        print(f"Ошибка CAN: {e}")
        return False

def validate_crc16(data):
    """Расчет CRC16 (MODBUS) для проверки целостности данных"""
    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_pid_value(bus, device_id, reg, value):
    """Отправка коэффициента PID на устройство"""
    # Формируем CAN ID для записи: (device_id << 4) | REG_WRITE
    can_id_write = (device_id << 4) | REG_WRITE
    
    # Упаковываем значение в байты (little-endian)
    float_bytes = struct.pack('<f', value)
    
    # Формируем часть данных (регистр + значение)
    data_part = [reg] + list(float_bytes)
    
    # Полные данные для расчета CRC: CAN ID + данные
    full_data_for_crc = list(can_id_write.to_bytes(2, 'little')) + data_part
    
    # Рассчитываем CRC и разбиваем на байты (little-endian)
    crc = validate_crc16(full_data_for_crc)
    crc_bytes = list(crc.to_bytes(2, 'little'))
    
    # Собираем итоговый пакет данных
    can_data = data_part + crc_bytes
    
    # Отправляем сообщение
    send_can_message(bus, can_id_write, can_data)

def main():
    # Запрос коэффициентов у пользователя
    try:
        p = float(input("Введите коэффициент P: "))
        i = float(input("Введите коэффициент I: "))
        d = float(input("Введите коэффициент D: "))
    except ValueError:
        print("Ошибка: Введите числовые значения.")
        return

    # Инициализация CAN-интерфейса
    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
    
    try:
        # Отправка коэффициентов с задержкой
        send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp, p)
        time.sleep(1)
        
        send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki, i)
        time.sleep(1)
        
        send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd, d)
        
    finally:
        # Завершение работы с шиной
        bus.shutdown()

if __name__ == "__main__":
    if len(sys.argv) != 2:
        print("Используйте python3 pid_set.py addr")
        sys.exit(1)
    main()