"""Change a Feetech servo's ID (EEPROM). Same flow as the old set_motor_id.py.

IMPORTANT: connect only ONE servo to the bus when running this — every servo that
answers to <old_id> would get changed, and re-using an <new_id> already on the bus
creates a collision.

Usage:
  python set_id.py <old_id> <new_id> [--port /dev/tty...]
"""
import argparse
import sys

from feetech_bus import Bus


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("old_id", type=int)
    ap.add_argument("new_id", type=int)
    ap.add_argument("--port", default=None)
    a = ap.parse_args()

    if not (0 <= a.new_id <= 253):
        sys.exit("new_id must be in 0..253 (254 is the broadcast id).")
    if a.old_id == a.new_id:
        sys.exit("old_id and new_id are the same.")

    bus = Bus(a.port) if a.port else Bus()
    try:
        if not bus.ping(a.old_id):
            sys.exit(f"Servo {a.old_id} not responding.")
        if bus.ping(a.new_id):
            sys.exit(f"ID {a.new_id} is already in use on the bus. Aborting.")

        print(f"About to change servo ID {a.old_id} -> {a.new_id}.")
        print("Make sure ONLY ONE servo is connected to the bus.")
        if input("Proceed? [y/N] ").strip().lower() != "y":
            print("Aborted.")
            return

        if bus.set_id(a.old_id, a.new_id):
            print(f"Done. Servo now responds at ID {a.new_id}.")
        else:
            sys.exit(f"Failed — servo not responding at new ID {a.new_id}.")
    finally:
        bus.close()


if __name__ == "__main__":
    main()