import can
import time
import struct
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x00
SET_PID_P = 3.6     
REG_WRITE = 0x8
REG_READ = 0x7
REG_ID = 0x30       #REG_MOTOR_POSPID_Kp
PID_P  = 0x01

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 receive_response(bus, timeout=1.0):
    print("Ожидание ответа")
    start_time = time.time()
    while time.time() - start_time < timeout:
        msg = bus.recv(timeout=0.1)
        if msg:
            print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
            return msg
    print("[Ошибка] Таймаут")
    return None

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

# Инициализация
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# Перевод float -> hex -> int
result = (struct.unpack('<I',struct.pack('<f', float(SET_PID_P)))[0])
result_bytes = result.to_bytes(4, byteorder='little')
# ======= 1. Отправляем команду изменить ID =======

# Весь буфер: id + команда + параметры
OLD_WITH_REG = (DEVICE_ID << 4) | REG_WRITE
id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))

# Важные части сообщения: address (id), команда, параметры
data_write = [REG_ID] + list(result_bytes)  # команда изменить PID_P

# Полностью собираем массив для CRC (включая id и команду)
full_data_for_crc = id_bytes + data_write

# Расчет CRC по всему пакету
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))

# Итоговый пакет: команда + параметры + CRC
packet_write = data_write + crc_bytes

print("Отправляем: команда изменить PID_p + CRC:", packet_write)
# Отправляем с `OLD_DEVICE_ID` в качестве адреса
send_can_message(bus, (DEVICE_ID << 4) | REG_WRITE, packet_write)

time.sleep(1.0)

# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======

# Теперь для запроса используем **уже новый id**
NEW_WITH_REG = (DEVICE_ID << 4) | REG_READ
current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little'))
data_read = [REG_ID, 0x00]

full_data_for_crc = current_id_bytes + data_read
crc = validate_crc16(full_data_for_crc)
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
packet_read = data_read + crc_bytes

print("Запрос на чтение ID + CRC (после смены):", packet_read)
send_can_message(bus, (DEVICE_ID << 4) | REG_READ, packet_read)

# ======= 3. Получение и проверка ответа =======

response = receive_response(bus)
if response:
    data = response.data
    if len(data) < 4:
        print("Ответ слишком короткий")
    else:
        id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
        #buff with id and data without CRC
        full_data = list(id_bytes) + list(data[:-2])
        print(f"Received full_data: {list(full_data)}")
        received_crc = int.from_bytes(data[-2:], byteorder='little')
        #calc CRC
        calc_crc = validate_crc16(full_data)
        if received_crc == calc_crc:
            if data[0] == int(REG_ID):
                kp_val = struct.unpack('<f', bytes(data[1:5]))[0]
                print(f"\nУСПЕХ! PID_P = {kp_val:.3f}")
            else:
                print(f"Некорректный ответ: {list(data)}")
        else:
            print("CRC не совпадает, данные повреждены.")
else:
    print("Нет ответа от устройства.")

bus.shutdown()