#!/usr/bin/python3
import can
import time
import argparse
import sys



IFACE_NAME = "gcan0"

def send_command(bus, command_id):
    """Sends a CAN message with the given command ID."""
    message = can.Message(
        arbitration_id=command_id,
        data=[], # No specific data needed for these simple commands
        is_extended_id=False
    )
    try:
        bus.send(message)
        print(f"Sent command: ID=0x{command_id:X}")
    except can.CanError as e:
        print(f"Error sending message: {e}")

def scan(bus, start, stop):
    for x in range(start, stop+1):
        send_command(bus, x)
        time.sleep(1)

def main():
    try:
        # Initialize the CAN bus interface
        bus = can.interface.Bus(channel=IFACE_NAME, interface='socketcan', receive_own_messages=False) # Set receive_own_messages if needed
        print(f"Successfully connected to {IFACE_NAME}.")
        while True:
            d = input("Direction: ")
            if d == "w":
                send_command(bus, 0x201)
            if d == "a":
                send_command(bus, 0x203)
            if d == "s":
                send_command(bus, 0x202)
            if d == "d":
                send_command(bus, 0x204)
            time.sleep(0.1)
    except OSError as e:
        print(f"Error connecting to CAN interface {IFACE_NAME}: {e}")
        print(f"Make sure the {IFACE_NAME} interface is up ('sudo ip link set up {IFACE_NAME}')")
        print("And that you have the necessary permissions.")
        sys.exit(1)
    except KeyboardInterrupt as ke:
        # Shutdown the bus connection cleanly
        bus.shutdown()
        print("Ctrl+c... exiting")
        sys.exit(0)
    except Exception as e:
        print(f"An unexpected error occurred during bus initialization: {e}")
        sys.exit(1)

    # Shutdown the bus connection cleanly
    bus.shutdown()
    print("CAN bus connection closed.")

if __name__ == "__main__":
    main()