import numpy as np
from scipy.optimize import minimize


# 目标函数
def objective(x):
    return 2 * (x[2] + x[4]) * x[5] + (x[3] + x[4]) * x[5]


# 非线性约束函数
def nonlin_constraints(x):
    R = x[5]
    theta1, theta2, k1, k2, phi = x[:5]

    r1 = (1.7 / (2 * np.pi)) * theta1
    r2 = -(1.7 / (2 * np.pi)) * theta2

    tan_phi_left = np.tan(phi)
    tan_phi_right = (r1 * np.sin(theta1) - 2 * R * np.sin(k1) - r2 * np.sin(theta2) - R * np.sin(k2)) / \
                    (r1 * np.cos(theta1) - 2 * R * np.cos(k1) - r2 * np.cos(theta2) - R * np.cos(k2))

    ceq1 = (theta1 * np.sin(theta1) - np.cos(theta1)) / (np.sin(theta1) + theta1 * np.cos(theta1)) - np.tan(k1)
    ceq3 = (theta2 * np.sin(theta2) - np.cos(theta2)) / (np.sin(theta2) + theta2 * np.cos(theta2)) - np.tan(k2)

    d = np.sqrt((r1 * np.cos(theta1) - 2 * R * np.cos(k1) - r2 * np.cos(theta2) - R * np.cos(k2)) ** 2 +
                (r1 * np.sin(theta1) - 2 * R * np.sin(k1) - r2 * np.sin(theta2) - R * np.sin(k2)) ** 2)

    ceq5 = d - 3 * R
    ceq6 = tan_phi_right - tan_phi_left

    ceq = [ceq1, ceq3, ceq5, ceq6]

    c = [
        k1 + phi - 2 * np.pi,
        k2 + phi - 2 * np.pi,
        r1 - 4.5,
        -r2 - 4.5,
        r1,
        -r2
    ]

    return np.array(c), np.array(ceq)


# 参数定义和初始猜测
x0 = [np.pi, 2 * np.pi, np.pi / 2, np.pi / 2, np.pi / 2, 1]

# 上下界定义
theta1_range = [2 * np.pi, 9 * np.pi]
theta2_range = [2 * np.pi, 9 * np.pi]
k_range = [0, 2 * np.pi]
phi_range = [0, 2 * np.pi]
R_range = [0, 3]

bounds = [
    theta1_range,
    theta2_range,
    k_range,
    k_range,
    phi_range,
    R_range
]

# 使用 scipy.optimize.minimize 进行非线性优化
cons = [
    {'type': 'eq', 'fun': lambda x: nonlin_constraints(x)[1]},
    {'type': 'ineq', 'fun': lambda x: -nonlin_constraints(x)[0]}
]

result = minimize(objective, x0, method='SLSQP', bounds=bounds, constraints=cons, options={'disp': True})

# 打印结果
print('Optimal Solution:')
print(f'theta1: {result.x[0]}')
print(f'theta2: {result.x[1]}')
print(f'k1: {result.x[2]}')
print(f'k2: {result.x[3]}')
print(f'phi: {result.x[4]}')
print(f'R: {result.x[5]}')
print(f'Minimum Objective Value: {result.fun}')