#SCRIPT TO SET THE TIMEOUT OF A DAMIAO MOTOR, by default motor comes configured with a timeout, this script removes the timeout 
# RUN python set_timeout.py --motor_id 7  # to disable timeout
import argparse
import time

from feathersdk.robot.motors.damiao.utils import (
    RawCanInterface,
    get_special_message_response,
    save_to_memory,
    write_special_message,
)


parser = argparse.ArgumentParser(
    description="Set timeout for Damiao motors. Timeout of 8000 keeps motor enabled even without communication."
)
parser.add_argument("--channel", type=str, default="can0", help="CAN interface channel (default: can0)")
parser.add_argument("--timeout", type=int, default=0, help="Timeout value in ms. Use 8000 to keep motor enabled without communication, 0 to disable timeout (default: 0)")
parser.add_argument("--motor_id", type=int, default=7, help="Motor ID to configure. If not specified, configures all motors [1-7]")

args = parser.parse_args()
can_interface = RawCanInterface(
    channel=args.channel,
    bustype="socketcan",
)

timeout = args.timeout

motor_id = args.motor_id
if motor_id < 0:
    motor_ids = [1, 2, 3, 4, 5, 6, 7]
else:
    motor_ids = [motor_id]

for motor_id in motor_ids:
    # Turn off motor before setting timeout
    # motor_off command: frame_id = motor_id, data = [0xFF] * 7 + [0xFD]
    frame_id = motor_id
    motor_off_data = [0xFF] * 7 + [0xFD]
    try:
        can_interface._send_message_get_response(frame_id, motor_off_data, max_retry=5)
        print(f"Motor {motor_id} turned off")
    except Exception as e:
        print(f"Warning: Could not turn off motor {motor_id}: {e}")
    
    print("#" * 30)
    print(f"processing motor {motor_id}, before setting timeout")
    for reg_name in ["id", "master_id", "timeout"]:
        try:
            info = get_special_message_response(can_interface, motor_id, reg_name)
            print(f"current setting: {reg_name} = {info}")
        except Exception as e:
            print(f"Warning: Could not read {reg_name} for motor {motor_id}: {e}")
        time.sleep(0.1)

    try:
        write_special_message(can_interface, motor_id, "timeout", timeout)
        print(f"Timeout written to motor {motor_id}: {timeout}")
        save_result = save_to_memory(can_interface, motor_id, "timeout")
        print(f"Timeout saved to motor {motor_id} memory")
    except Exception as e:
        print(f"Error: Could not write/save timeout for motor {motor_id}: {e}")
        continue

    print("after setting timeout")
    for reg_name in ["id", "master_id", "timeout"]:
        try:
            info = get_special_message_response(can_interface, motor_id, reg_name)
            print(f"current setting: {reg_name} = {info}")
        except Exception as e:
            print(f"Warning: Could not read {reg_name} for motor {motor_id}: {e}")
        time.sleep(0.1)
    
    # Clear any remaining messages
    for _ in range(4):
        can_interface.try_receive_message(motor_id)

can_interface.close()
print("Done!")
