import can
import struct
import time
import sys

# Function to send the target speed
def send_target_speed(bus, target_speed):
    msg = can.Message()
    msg.arbitration_id = 1  # Message ID
    msg.is_extended_id = False
    msg.dlc = 5  # Message length
    msg.data = bytearray([ord('V')] + list(struct.pack('<f', target_speed)))  # 'V' for the command identifier, followed by the speed in float format

    try:
        bus.send(msg)
        print(f"Sent message with target speed: {target_speed} rad/s")
    except can.CanError:
        print("Message failed to send")

# Function to send the motor enable/disable command
def send_motor_enable(bus, enable):
    """
    Sends a command to enable or disable the motor.
    
    :param bus: The CAN bus
    :param enable: 1 to enable the motor, 0 to disable it
    """
    msg = can.Message()
    msg.arbitration_id = 1  # Message ID
    msg.is_extended_id = False
    msg.dlc = 2  # Message length (flag + 1 byte of data)
    msg.data = bytearray([ord('E'), enable])  # 'E' for the command, followed by 0 or 1

    try:
        bus.send(msg)
        state = "enabled" if enable else "disabled"
        print(f"Sent message to {state} motor")
    except can.CanError as e:
        print(f"Message failed to send: {e}")
        sys.exit(1)  # Exit the program on failure

    send_target_speed(bus,0.0)

def main():
    # CAN interface setup
    bus = None  # Define outside the try block for proper shutdown
    try:
        bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
        print("CAN bus initialized.")

        while True:
            user_input = input("Enter target speed: ")
            if user_input.lower() == 'exit':
                print("Exiting...")
                break
            try:
                target_speed = float(user_input)
                send_target_speed(bus, target_speed)
            except ValueError:
                print("Invalid input. Please enter a valid number.")

        # Disable motor before exiting
        send_motor_enable(bus, 0)
        print("Motor disabled.")
    
    except Exception as e:
        print(f"Error initializing1 CAN bus: {e}")
        sys.exit(1)

    finally:
        if bus is not None:
            bus.shutdown()
            print("CAN bus shut down.")

if __name__ == '__main__':
    main()