import matplotlib.pyplot as plt
import numpy as np

kPitch = 1.7
kAlpha = kPitch / (2 * np.pi)
kCriticalRadius = 4.5

theta_max = (kCriticalRadius) / kAlpha + 2 * 2 * np.pi
kPlotingRadius = theta_max * kAlpha

kCriticalTheta = 2.86 / ((2 / 3) * kAlpha)
# 生成角度数组
theta = np.linspace(kCriticalTheta, theta_max, 1000)

# 根据等距螺旋方程计算半径
r1 = kAlpha * theta
r2 = kAlpha * theta  # 半径保持正值

# 为第二只螺旋添加相位偏移
theta2 = theta + np.pi  # 角度偏移π

# 创建图形
plt.figure()
ax = plt.subplot(projection='polar')  # 使用极坐标系

# 绘制第一只螺旋
ax.plot(theta, r1)

# 绘制中心对称的另一只螺旋
ax.plot(theta2, r2)

# 绘制半径为4.5的圆
circle_theta = np.linspace(0, 2 * np.pi, 1000)
circle_r = np.full_like(circle_theta, kCriticalRadius)
ax.plot(circle_theta, circle_r, linestyle='--')

point_A_cartesian = (kAlpha * kCriticalTheta * np.cos(kCriticalTheta), kAlpha * kCriticalTheta * np.sin(kCriticalTheta))
point_B_cartesian = (-kAlpha * kCriticalTheta * np.cos(kCriticalTheta),
                     -kAlpha * kCriticalTheta * np.sin(kCriticalTheta))
kPhi = np.arctan(kCriticalTheta)
r = (1 / 3) * kAlpha * np.sqrt(1 + kCriticalTheta**2)
dx, dy = point_A_cartesian[0] - point_B_cartesian[0], point_A_cartesian[1] - point_B_cartesian[1]
angle = np.arctan2(dy, dx)
dx, dy = np.cos(angle - (0.5 * np.pi - kPhi)), np.sin(angle - (0.5 * np.pi - kPhi))
point_C1_cartesian = (point_A_cartesian[0] - 2 * r * dx, point_A_cartesian[1] - 2 * r * dy)
point_C2_cartesian = (point_B_cartesian[0] + 1 * r * dx, point_B_cartesian[1] + 1 * r * dy)
radius_of_C1 = 2 * r
radius_of_C2 = 1 * r


# 定义用于绘制圆的函数
def draw_circle(ax, center, radius, num_points, beg_angle, span_angle):
  t = np.linspace(beg_angle, beg_angle + span_angle, num_points)
  x = center[0] + radius * np.cos(t)
  y = center[1] + radius * np.sin(t)
  r, theta = np.sqrt(x**2 + y**2), np.arctan2(y, x)
  ax.plot(theta, r)


# 绘制圆C1
draw_circle(ax, point_C1_cartesian, radius_of_C1, 100, angle + 0.5 * np.pi - kPhi - np.pi, 2 * kPhi)

# 绘制圆C2
draw_circle(ax, point_C2_cartesian, radius_of_C2, 100, angle + 0.5 * np.pi - kPhi, 2 * kPhi)

print(f"Total length={6*r*kPhi}")
print(f"kCriticalTheta={kCriticalTheta}")

x_ticks = np.arange(-int(kPlotingRadius) - 1, int(kPlotingRadius) + 1, 1)
y_ticks = np.arange(-int(kPlotingRadius) - 1, int(kPlotingRadius) + 1, 1)
X, Y = np.meshgrid(x_ticks, y_ticks)
X = X.flatten()
Y = Y.flatten()

# 将网格点转换为极坐标
r_grid = np.sqrt(X**2 + Y**2)
theta_grid = np.arctan2(Y, X)

# 仅绘制半径不超过kPlotingRadius的点
valid_points = r_grid <= kPlotingRadius
ax.scatter(theta_grid[valid_points], r_grid[valid_points], color='grey', s=10)  # 灰色小点

plt.title("The Moving Path")

# 显示图像
plt.show()