"""从当前高度下降 20 cm; 执行时须有至少 0.2 m 下降空间.

机构仍须遵循可靠支撑的机械最低位置启动前提.

高度单位 m, 绝对角速度单位 rad/s; speed_percent 是无单位速度比例.
单次位置与角速度命令见文档中的升降进阶控制; 本例使用自动检查到位的 move_to.
"""
from libark.robot.hw1.lift import Lift

SPEED_PERCENT = 100.0  # 相对允许角速度上限的比例, 不是绝对角速度.


def main() -> None:
    with Lift() as lift:
        lift.enable()
        try:
            height = lift.get_height()
            if height is None:
                raise RuntimeError("升降反馈缺失,请联系技术支持")
            target = min(height - 0.2, lift.max_height_m)
            print("高度 m:", height)
            print("目标高度 m:", target)
            print("速度比例 %:", SPEED_PERCENT, ";绝对角速度使用 rad/s")
            print("运动完成:", lift.move_to(target, speed_percent=SPEED_PERCENT, timeout_s=100.0))
            print("高度 m:", lift.get_height())

            # for i in range(20):
            #     print("第", i, "次")
            #     print("高度 m:", lift.get_height())
            #     lift.move_to(1.031587950967, speed_percent=SPEED_PERCENT, timeout_s=100.0)

            #     time.sleep(1)
            #     print("高度 m:", lift.get_height())
            #     lift.move_to(0, speed_percent=SPEED_PERCENT, timeout_s=100.0)

            # 烤机测试 0-1.034587950967 之间来回 随机高度
            # for i in range(100):
            #     print("第", i, "次")
            #     random_height = random.uniform(0, 1.034587950967)
            #     print("随机高度 m:", random_height)
            #     lift.move_to(random_height, speed_percent=SPEED_PERCENT, timeout_s=100.0)
            #     time.sleep(1)
            #     print("高度 m:", lift.get_height())

        except BaseException:
            lift.stop()
            raise


if __name__ == "__main__":
    main()