import can
import time
import struct
import sys
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = int(sys.argv[1])        # ID устройства по умолчанию
REG_READ = 0x7          # Код команды чтения
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_read_request(bus, device_id, register):
    """Отправка запроса на чтение регистра"""
    can_id = (device_id << 4) | REG_READ
    data_part = [register, 0x00]
    
    # Расчет CRC для CAN ID (2 байта) + данные
    full_data_for_crc = list(can_id.to_bytes(2, 'little')) + data_part
    crc = validate_crc16(full_data_for_crc)
    crc_bytes = list(crc.to_bytes(2, 'little'))
    
    # Формирование итогового пакета
    packet = data_part + crc_bytes
    send_can_message(bus, can_id, packet)

def receive_pid_response(bus, timeout=1.0):
    """Получение и проверка ответа с PID-значением"""
    start_time = time.time()
    while time.time() - start_time < timeout:
        msg = bus.recv(timeout=0.1)
        if msg and msg.arbitration_id == DEVICE_ID:
            print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
            
            if len(msg.data) < 8:
                print("Ошибка: Слишком короткий ответ")
                return None
                
            # Извлечение данных и CRC
            data = msg.data
            received_crc = int.from_bytes(data[-2:], byteorder='little')
            
            # Подготовка данных для проверки CRC
            id_bytes = msg.arbitration_id.to_bytes(1, 'little')
            full_data = list(id_bytes) + list(data[:-2])
            
            # Проверка CRC
            calc_crc = validate_crc16(full_data)
            if calc_crc != received_crc:
                print(f"Ошибка CRC: ожидалось 0x{calc_crc:04X}, получено 0x{received_crc:04X}")
                return None
                
            # Извлечение float значения
            try:
                value = struct.unpack('<f', bytes(data[1:5]))[0]
                return value
            except struct.error:
                print("Ошибка распаковки float")
                return None
                
    print("Таймаут ожидания ответа")
    return None

def main():
    """Основная логика чтения PID-коэффициентов"""
    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
    
    try:
        # Чтение коэффициентов с задержкой
        print("\nЧтение Kp...")
        send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp)
        kp = receive_pid_response(bus)
        if kp is not None: 
            print(f"Текущий Kp: {kp:.3f}")
        
        time.sleep(1)
        
        print("\nЧтение Ki...")
        send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki)
        ki = receive_pid_response(bus)
        if ki is not None:
            print(f"Текущий Ki: {ki:.3f}")
        
        time.sleep(1)
        
        print("\nЧтение Kd...")
        send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd)
        kd = receive_pid_response(bus)
        if kd is not None:
            print(f"Текущий Kd: {kd:.3f}")
            
    finally:
        bus.shutdown()

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