"""Zero + direction calibration for ONE Feetech servo, done by hand.

Two captures:
  1) Move the servo to its URDF q=0 pose, press Enter -> INST_OFSCAL sets that
     physical position as tick 2048 (the joint zero), persisted in the servo EEPROM.
  2) Move the servo in the URDF-NEGATIVE direction for that joint, press Enter ->
     the tool reads which way the tick moved and records the sign (+1/-1), so later
     `ticks_to_rad(pos, sign)` maps encoder ticks to URDF radians.

Torque is disabled up front so you can move the servo freely. Result is merged into
calib/<config>.json keyed by servo id (run once per servo to build up a whole robot).

Usage:
  python calibrate_servo.py <servo_id> --config gripper [--joint jaw] [--port /dev/tty...]
"""
import argparse
import json
import os
import sys

from feetech_bus import Bus, CENTER_TICK
from configs import CONFIGS, calib_path

MIN_DELTA_TICKS = 40   # ~3.5 deg — require a clear move so the sign is unambiguous


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("servo_id", type=int)
    ap.add_argument("--config", default="gripper")
    ap.add_argument("--joint", default=None,
                    help="URDF joint this servo drives (default: the config's default_joint)")
    ap.add_argument("--port", default=None)
    a = ap.parse_args()

    cfg = CONFIGS.get(a.config, {})
    joint = a.joint or cfg.get("joint_map", {}).get(a.servo_id) or cfg.get("default_joint")
    if joint is None:
        sys.exit(f"No --joint given and config '{a.config}' has no mapping for servo {a.servo_id}.")

    bus = Bus(a.port) if a.port else Bus()
    try:
        if not bus.ping(a.servo_id):
            sys.exit(f"Servo {a.servo_id} not responding on the bus.")
        bus.torque(a.servo_id, False)
        print(f"Servo {a.servo_id} torque OFF — you can move it by hand.\n")

        input(f"1) Move servo {a.servo_id} to its ZERO pose (URDF q=0 for '{joint}'), then press Enter... ")
        raw = bus.read_pos(a.servo_id)
        if not bus.set_zero_here(a.servo_id):
            sys.exit("INST_OFSCAL failed — servo did not accept the zero.")
        z = bus.read_pos_settle(a.servo_id)
        print(f"   zeroed: was {raw}, now reads {z} (target {CENTER_TICK}).\n")

        while True:
            input(f"2) Move servo {a.servo_id} in the NEGATIVE '{joint}' direction (per the URDF/sim), "
                  f"then press Enter... ")
            t = bus.read_pos(a.servo_id)
            if t is None:
                print("   read failed, try again.")
                continue
            d = t - CENTER_TICK
            if abs(d) < MIN_DELTA_TICKS:
                print(f"   only {d:+d} ticks of motion — move it further in the negative "
                      f"direction and press Enter again.")
                continue
            break

        sign = +1 if d < 0 else -1
        print(f"   negative pose tick={t} (delta {d:+d})  ->  sign = {sign:+d}")

        path = calib_path(a.config)
        os.makedirs(os.path.dirname(path), exist_ok=True)
        data = json.load(open(path)) if os.path.exists(path) else {}
        data[str(a.servo_id)] = {
            "joint": joint, "sign": sign, "zero_tick": CENTER_TICK,
            "raw_zero_before_cal": raw, "neg_sample_tick": t,
        }
        json.dump(data, open(path, "w"), indent=2)
        print(f"\nSaved -> {path}\n  servo {a.servo_id}: joint='{joint}'  sign={sign:+d}  "
              f"(zero stored in servo EEPROM)")
    finally:
        bus.close()


if __name__ == "__main__":
    main()