import can
import time
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
REG_WRITE = 0x8        # Код команды чтения
REG_ID = 0x55          # Адрес регистра с Firmware Update

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

# Инициализация CAN-интерфейса
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')

# ======= 1. Запрос текущего ID устройства =======

# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
can_id_read = (OLD_DEVICE_ID << 4) | REG_WRITE

# Данные для запроса: [регистр, резервный байт]
data_read = [REG_ID, 0x00]

# Формируем полные данные для расчета CRC:
# - CAN ID разбивается на 2 байта (little-endian)
# - Добавляем данные запроса
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read

# Рассчитываем CRC и разбиваем на байты (little-endian)
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, 'little'))

# Собираем итоговый пакет: данные + CRC
packet_read = data_read + crc_bytes

print("Переход в boot режим", packet_read)
send_can_message(bus, can_id_read, packet_read)

bus.shutdown()

if __name__ == "__main__":
    import sys
    if len(sys.argv) != 2:
        print("Использование: python3 firmware_test.py address")
        sys.exit(1)