import can
import struct
import time

def process_can_message(msg):
    if msg.dlc == 5:  # Check the message length
        print(f"Received message with ID: {msg.arbitration_id}")
        print(f"Data: {msg.data}")

        # The first byte determines the data type (flag)
        flag = chr(msg.data[0])
        
        if flag == 'A':  # Angle
            angle_bytes = msg.data[1:5]
            angle = struct.unpack('<f', bytes(angle_bytes))[0]
            print(f"Angle: {angle} degrees")
        elif flag == 'V':  # Velocity
            velocity_bytes = msg.data[1:5]
            velocity = struct.unpack('<f', bytes(velocity_bytes))[0]
            print(f"Velocity: {velocity} rad/s")
        elif flag == 'E' and msg.dlc >= 2:  # Enable/Disable
            enabled = msg.data[1]  # Expecting 1 byte (0 or 1)
            print(f"Enabled: {bool(enabled)}")
        else:
            print(f"Unknown flag: {flag}")
    else:
        print(f"Received message with unexpected length: {msg.dlc}")

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:
            msg = bus.recv()
            if msg:
                process_can_message(msg)

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

if __name__ == '__main__':
    receive_can_messages()