import math


def inverseF(x, y, l1, l2, l3, l4, l5):
    """
    五连杆机构逆向运动学函数（Python 实现）

    输入：
        x, y: 末端执行器坐标
        l1~l5: 各杆长度

    输出：
        theta1, theta2: 两个主动关节角度（弧度）
    """
    Xc = x
    Yc = y

    # ===== 左侧链路（l1, l2）=====
    numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2
    denominator_left = 2 * l1 * l2
    cosfoai_12 = numerator_left / denominator_left

    if cosfoai_12 > 1 or cosfoai_12 < -1:
        raise ValueError("目标点超出工作空间！左侧无解。")
    foai_12 = 2 * math.pi - math.acos(cosfoai_12)

    # 求第一个电机角度 foai_01
    numerator_foai01 = l2 * Yc * math.sin(foai_12) + Xc * (l2 * math.cos(foai_12) + l1)
    denominator_foai01 = (l2 * math.cos(foai_12) + l1) ** 2 + (l2 * math.sin(foai_12)) ** 2

    if denominator_foai01 == 0:
        raise ZeroDivisionError("分母为零，无法计算左侧角度。")

    cosfoai_01 = numerator_foai01 / denominator_foai01
    if cosfoai_01 > 1 or cosfoai_01 < -1:
        raise ValueError("cosfoai_01 超出 [-1, 1]，左侧无解。")
    foai_01 = math.acos(cosfoai_01)

    # ===== 右侧链路（l3, l4）=====
    Xc_shifted = Xc - l5
    numerator_right = Xc_shifted ** 2 + Yc ** 2 - l3 ** 2 - l4 ** 2
    denominator_right = 2 * l3 * l4
    cosfoai_34 = numerator_right / denominator_right

    if cosfoai_34 > 1 or cosfoai_34 < -1:
        raise ValueError("目标点超出工作空间！右侧无解。")
    foai_34 = 2 * math.pi - math.acos(cosfoai_34)

    A = l5 - Xc
    B = l3 * math.sin(foai_34)
    C = l4 + l3 * math.cos(foai_34)

    if B == 0 and C == 0:
        raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。")

    try:
        foai_t = math.acos(B / math.sqrt(B ** 2 + C ** 2))
        foai_40 = foai_t - math.asin(A / math.sqrt(B ** 2 + C ** 2))
    except:
        raise ValueError("右侧三角函数计算失败，请检查输入是否合法。")

    # 转换为角度再转回弧度
    theta1_deg = math.degrees(foai_01)
    theta2_deg = 180 - math.degrees(foai_40)

    theta1 = math.radians(theta1_deg)
    theta2 = math.radians(theta2_deg)

    return theta1, theta2