import can
import struct
import time


REG_READ = 0x7
DEVICE_ID = 0x00          # Старый ID устройства
REG_POS = 0x72             # Предположим, что команда записи позиции — 0x2
CAN_INTERFACE = 'can0'


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 process_can_message(bus):
    # ID и команда
    arbitration_id = (DEVICE_ID << 4) | REG_READ
    id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))

    # Команда и параметры
    data_read = [REG_POS] + list(struct.pack('<f', 0))

    # Для CRC — весь пакет: id + команда + параметры
    full_data_for_crc = id_bytes + data_read
    crc = validate_crc16(full_data_for_crc)
    crc_bytes = list(crc.to_bytes(2, byteorder='little'))

    # Итоговый пакет
    packet = data_read + crc_bytes

    # Создание и отправка сообщения
    msg = can.Message(
        arbitration_id=arbitration_id,
        is_extended_id=False,
        data=packet
    )

    try:
        bus.send(msg)
    except can.CanError:
        print("Ошибка отправки сообщения")


def receive_response(bus, timeout=2.0):
    """Ожидание ответа"""
    start_time = time.time()
    while time.time() - start_time < timeout:
        msg = bus.recv(timeout=timeout)
        data_bytes = msg.data[1:5]  # берём первые 4 байта
        print(data_bytes)
        angle = struct.unpack('<f', bytes(data_bytes))[0]  # '<f' = little-endian float
        # angle = int.from_bytes(data_bytes, byteorder='little', signed=True)  # или 'big'
        if msg:
            print(f"[Прием] Angle: 0x{msg.arbitration_id:03X}, Данные: {angle}, RAW: {bytes(msg.data)}")
            return msg
    print("[Ошибка] Таймаут")
    return None

def receive_can_messages():
    try:
        # Connect to the CAN bus
        bus = can.interface.Bus(channel='can0', bustype='socketcan')

        print("Waiting for messages on the CAN bus...")

        while True:
            process_can_message(bus)
            response = receive_response(bus)
            if response:
                data = response.data
                if len(data) < 4:
                    print("Ответ слишком короткий")
                else:
                    received_crc = int.from_bytes(data[-2:], byteorder='little')
                    print("Полученный CRC: ", received_crc)
                    # Расчет CRC по всему пакету без CRC
                    calc_crc = validate_crc16(data[:-2])
                    if received_crc == calc_crc:
                        if data[0] == REG_POS:
                            print("\nУСПЕХ!")
                        else:
                            print(f"Некорректный ответ: {list(data)}")
                    else:
                        print("CRC не совпадает, данные повреждены.")
            else:
                print("Нет ответа от устройства.")

    except KeyboardInterrupt:
        print("\nExiting program...")
    except Exception as e:
        print(f"Error: {e}")

if __name__ == '__main__':
    receive_can_messages()