from test_pin import KinematicsSolver as controller
from rm75_mjc import MuJoCoPositionController

import time
from pathlib import Path

def demo_position_control():
    """Demonstrate pure position control"""

    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"

    if not Path(urdf_path).exists():
        print(f"Error: URDF not found at {urdf_path}")
        return

    print("=" * 60)
    print("Pure Position Control Demo")
    print("=" * 60)

    # Create controller
    robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
    robot.start()
    time.sleep(1)

    print("\n[Test 1] Move joint 1 to 45 degrees")
    robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
    robot.wait_until_reached()
    robot.print_state()
    time.sleep(0.5)

    print("\n[Test 2] Move joint 2 to -30 degrees")
    robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
    robot.wait_until_reached()
    robot.print_state()
    time.sleep(0.5)

    print("\n[Test 3] Move multiple joints simultaneously")
    robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
    robot.wait_until_reached()
    robot.print_state()
    time.sleep(0.5)

    print("\n[Test 4] Return home")
    robot.send_command([0, 0, 0, 0, 0, 0, 0])
    robot.wait_until_reached()
    robot.print_state()

    print("\n" + "=" * 60)
    print("✓ All tests passed! Robot is stable and controllable.")
    print("=" * 60)
    print("\nInteractive mode - close viewer to exit")

    try:
        while robot.viewer and robot.viewer.is_running():
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass

    robot.stop()

def main():

    demo_position_control()

    # kine_node = controller()
    # kine_node.loop_run()
    # print("main get returned kine_node")


if __name__ == "__main__":
    main()