import time

# 倒料函数
# 工具坐标tool 修改为 夹具
# 倒料时的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
def start_drop(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100):
    # 1、让机器臂第六轴旋转180° (倒料)
    # 获取倒料点 关节坐标，准备倒料
    result = robot_client.GetActualJointPosDegree()
    # result = robot.GetActualJointPosRadian()
    if isinstance(result, tuple) and len(result) == 2:
        # 成功：错误码和关节坐标（度数）
        _, joint_pos = result
        print(f"倒料点关节坐标：{joint_pos}")
    else:
        # 失败：result 为错误码
        print(f"start_drop错误: 获取倒料点关节坐标失败，错误码：{result}")
        return False

    # 修改第六轴关节坐标角度，准备旋转
    new_joint_pos = joint_pos.copy()
    # 修改第六轴角度, 加上旋转角度
    new_joint_pos[-1] = new_joint_pos[-1] + rot_angle
    # print("旋转前关节坐标：", joint_pos)
    # print("确认旋转后关节坐标：", new_joint_pos)

    # 旋转第六轴，倒料
    errno = robot_client.MoveJ(
        new_joint_pos, tool=tool, user=user, vel=drop_vel
    )
    if errno == 0:
        print(f"执行坐标: {new_joint_pos} 成功")
        print("倒料成功!!!!!")
    else:
        print(f"start_drop错误: 旋转第六轴失败，错误码: {errno}")
        return False

    # 确认倒料完成
    time.sleep(2)
    # 这里需要抖动?

    # 第六轴复位
    errno = robot_client.MoveJ(joint_pos, tool=tool, user=user, vel=drop_vel)
    if errno == 0:
        print(f"执行坐标: {joint_pos} 成功")
        print("第六轴复位成功!!!!!")
    else:
        print(f"start_drop错误: 第六轴复位失败，错误码: {errno}")
        return False
    
    return True # 倒料成功，包括 旋转 和 复原

# 倒料旋转函数（只旋转不复原）
# 工具坐标tool 修改为 夹具
# 倒料时的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
def start_drop_rotate(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100, global_vel = 40):
    # 0. 设置全局速度为 100%, 达到最快的倒料旋转速度
    robot_client.SetSpeed(100)

    # 1、让机器臂第六轴旋转180° (倒料)
    # 获取倒料点 关节坐标，准备倒料
    result = robot_client.GetActualJointPosDegree()
    # result = robot.GetActualJointPosRadian()
    if isinstance(result, tuple) and len(result) == 2:
        # 成功：错误码和关节坐标（度数）
        _, joint_pos = result
        print(f"倒料旋转点关节坐标：{joint_pos}")
    else:
        # 失败：result 为错误码
        print(f"start_drop_rotate错误: 获取倒料点关节坐标失败，错误码：{result}")
        return False

    # 修改第六轴关节坐标角度，准备旋转
    new_joint_pos = joint_pos.copy()
    # 修改第六轴角度, 加上旋转角度
    new_joint_pos[-1] = new_joint_pos[-1] + rot_angle
    # print("旋转前关节坐标：", joint_pos)
    # print("确认旋转后关节坐标：", new_joint_pos)

    # 旋转第六轴，倒料
    errno = robot_client.MoveJ(
        new_joint_pos, tool=tool, user=user, vel=drop_vel
    )

    # 2. 设置回原来的 全局速度
    robot_client.SetSpeed(global_vel)

    # 旋转倒料状态
    if errno == 0:
        print(f"执行坐标: {new_joint_pos} 成功")
        print("倒料旋转成功!!!!!")
    else:
        print(f"start_drop_rotate错误: 旋转第六轴失败，错误码: {errno}")
        return False

    # 确认倒料完成
    time.sleep(1)
    # 这里需要抖动?

    return True # 倒料旋转成功

# 倒料复原函数（复原, 第六轴旋转回去）
# 工具坐标tool 修改为 夹具
# 复原的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
def start_drop_reset(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100, global_vel = 40):
    # 0. 设置全局速度为 100%, 达到最快的倒料旋转速度
    robot_client.SetSpeed(100)

    # 1、让机器臂第六轴旋转180° (复原)
    # 获取复原点 关节坐标，准备复原
    result = robot_client.GetActualJointPosDegree()
    # result = robot.GetActualJointPosRadian()
    if isinstance(result, tuple) and len(result) == 2:
        # 成功：错误码和关节坐标（度数）
        _, joint_pos = result
        print(f"倒料复原点关节坐标：{joint_pos}")
    else:
        # 失败：result 为错误码
        print(f"start_drop_reset错误: 获取倒料点关节坐标失败，错误码：{result}")
        return False

    # 修改第六轴关节坐标角度，准备旋转
    new_joint_pos = joint_pos.copy()
    # 修改第六轴角度, 减去旋转角度复原
    new_joint_pos[-1] = new_joint_pos[-1] - rot_angle 
    # print("旋转前关节坐标：", joint_pos)
    # print("确认旋转后关节坐标：", new_joint_pos)

    # 旋转第六轴，复原
    errno = robot_client.MoveJ(
        new_joint_pos, tool=tool, user=user, vel=drop_vel
    )

    # 2. 设置回原来的 全局速度
    robot_client.SetSpeed(global_vel)

    if errno == 0:
        print(f"执行坐标: {new_joint_pos} 成功")
        print("倒料复原成功!!!!!")
    else:
        print(f"start_drop_reset错误: 复原第六轴失败，错误码: {errno}")
        return False
    
    return True # 倒料复原成功