import can
import time
import sys
import struct

# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x69  # Текущий ID устройства
REG_READ = 0x7
REG_WRITE = 0x8
DATA_TYPE_ANGLE = 0x03
DATA_TYPE_VELOCITY = 0x04
DATA_TYPE_TORQUE = 0x05

# CRC функция (аналогичная устройству)
def validate_crc16(data):
    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_can_message(bus, can_id, data):
    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):
    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 test_simplefoc_else_block():
    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
    
    # 1. Установка типа данных (DATA_TYPE_ANGLE)
    can_id_write = (DEVICE_ID << 4) | REG_WRITE
    data_set_type = [DATA_TYPE_ANGLE, 0x00]
    full_data = list(can_id_write.to_bytes(2, 'little')) + data_set_type
    crc = validate_crc16(full_data)
    crc_bytes = list(crc.to_bytes(2, 'little'))
    packet = data_set_type + crc_bytes
    send_can_message(bus, can_id_write, packet)
    time.sleep(0.1)  # Ожидание обработки
    
    # 2. Отправка SimpleFOC сообщения (угол)
    target_angle = 45.0
    angle_bytes = struct.pack('<f', target_angle)
    can_id_simplefoc = (DEVICE_ID << 4) | 0x01  # Не REG_READ/REG_WRITE
    payload = [0x00] + list(angle_bytes) + [0x00]  # [type placeholder, angle, padding]
    
    # Расчет CRC
    full_data_sf = list(can_id_simplefoc.to_bytes(2, 'little')) + payload
    crc_sf = validate_crc16(full_data_sf)
    payload += list(crc_sf.to_bytes(2, 'little'))
    
    # Отправка
    print("\nТест SimpleFOC (блок else):")
    send_can_message(bus, can_id_simplefoc, payload)
    
    # 3. Проверка ответа (отправка угла + установка нового угла)
    response = receive_response(bus)
    if response:
        # Проверка структуры ответа
        if response.data[0] == ord('A'):
            print("Успех: Отправлен текущий угол")
        else:
            print("Ошибка: Неверный тип ответа")
    else:
        print("Ошибка: Нет ответа от устройства")

    # 4. Проверка установки нового угла (интеграционно)
    # ... (может требовать дополнительной проверки на устройстве)
    
    bus.shutdown()

if __name__ == "__main__":
    test_simplefoc_else_block()