import numpy as np
import random
import math
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
n = 35
init1x = np.zeros(n)
init1y = np.zeros(n)
initx = np.zeros(n)
inity = np.zeros(n)
initz = np.zeros(n)
finalx = np.zeros(n)
finaly = np.zeros(n)
finalz = np.zeros(n)
startx = np.zeros(n)
starty = np.zeros(n)
startz = np.zeros(n)
midx = np.zeros(n)
midy = np.zeros(n)
midz = np.zeros(n)
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
for i in range(0, n):
    # initx[i] = random.random() * 100
    # inity[i] = random.random() * 100
    # initz[i] = random.random() * 100
    # finalx[i] = random.random() * 100
    # finaly[i] = random.random() * 100
    # finalz[i] = random.random() * 100
    # initx[i] = 30*random.random() +30
    # inity[i] = 30*random.random() +30
    # initz[i] = 0
    startx[i] = 2+i*40/n
    starty[i] = 2-abs(i-17)*20/17
    startz[i] = 75
    initx[i] = 2+i*40/n
    initz[i] = 75
    inity[i] = 22-abs(i-17)*20/17
    midx[i] = 5+3*int(i%7)
    midy[i] = 60
    midz[i] = 75+3*int(i/7)
    # finalx[i] = 2+i*40/n
    # finalz[i] = 70
    # finaly[i] = 22-abs(i-22)*20/22
    if i < 17:
        psai = 2*math.pi*i/22
        finalx[i] = 80
        finaly[i] = 80+10*math.cos(psai)
        finalz[i] = 80+10*math.sin(psai)
    else:
        psai = 2*math.pi*(i-22)/22
        finalx[i] = 80+10*math.cos(psai)
        finaly[i] = 80
        finalz[i] = 80+10*math.sin(psai)
    # initx[i] = 20 + 8 * math.cos(math.pi * 2 * i / n)
    # inity[i] = 80 + 8 * math.sin(math.pi * 2 * i / n)
    # finalx[i] = 30 + i * 10 / n
    # finaly[i] = 15 + i * 15 / n
    # initx[i] = 30 + i * 10 / n
    # inity[i] = 15 + i * 15 / n
    # finalx[i] = 85-15*abs(i-7)/7
    # finaly[i] = 60-40*i/n
    # initx[i] = 10 + 2 * i * 11 / n
    # inity[i] = 20  #+ 10 * math.sin(math.pi * 2 * i / n)
    # finalx[i] = 50 - 2 * i * 11 / n
    # finaly[i] = 65
    # if i <10:
    #     initx[i] = random.random()*30
    # else:
    #     initx[i] = random.random()*30 + 70
    # inity[i] = random.random()*30
    # finalx[i] = 50 + 10 * math.cos(math.pi * 2 * i / n)
    # finaly[i] = 80 + 10 * math.sin(math.pi * 2 * i / n)
placexs = []
placeys = []
placezs = []
tmpx = []
tmpy = []
tmpz = []
a1 = 0.5/math.sqrt(5)
for t in range(0,40):
    for cnt in range(0,n):
        placexs.append(startx[cnt])
        placeys.append(starty[cnt]+0.5*t)
        placezs.append(startz[cnt])
        
for t in range(0,44):
    for cnt in range(0,n):        
        tmpx.append(midx[cnt]+a1*t)
        tmpy.append(midy[cnt]+2*a1*t)
        tmpz.append(midz[cnt])
    
xplace1 = np.loadtxt("D:/wsf2x.csv",delimiter=",", dtype=float)
xplace2 = np.loadtxt("D:/wsf1x.csv",delimiter=",", dtype=float)

yplace1 = np.loadtxt("D:/wsf2y.csv",delimiter=",", dtype=float)
yplace2 = np.loadtxt("D:/wsf1y.csv",delimiter=",", dtype=float)

zplace1 = np.loadtxt("D:/wsf2z.csv",delimiter=",", dtype=float)
zplace2 = np.loadtxt("D:/wsf1z.csv",delimiter=",", dtype=float)
print(yplace1[14,20])
placex = []
placey = []
placez = []
xplace = np.hstack((xplace1,xplace2))
yplace = np.hstack((yplace1,yplace2))
zplace = np.hstack((zplace1,zplace2))
for i in range(0, n):
    for j in range(0, 4000):
        if (xplace[i, j]) != 0:
            placex.append(xplace[i, j])
            placey.append(yplace[i, j])
            placez.append(zplace[i, j])
plt.title("无人机轨迹散点图")

# plt.plot([70,80],[35,55])
ax = plt.axes(projection='3d')

z = np.linspace(0, 80, 1000)
theta1 = np.linspace(0,2*np.pi,30)
fai = np.linspace(0,2*np.pi,30)
theta1,fai = np.meshgrid(theta1,fai)
xr = 55+6*np.cos(theta1)*np.sin(fai)
yr = 80+6*np.sin(theta1)*np.sin(fai)
zr = 65+6*np.cos(fai)
ax.plot_surface(xr, yr, zr,cmap='viridis', edgecolor='none')
#调用 ax.plot3D创建三维线图
# x = 50+7.5*np.cos(theta)
# y = 60+7.5*np.sin(theta)

# ax.plot_surface(x, y, z,cmap='viridis', edgecolor='none')

# x1 = np.linspace(0,40,100)
# z1 = np.linspace(0,60,100)
# x1,z1 = np.meshgrid(x1,z1)
# y1 = -0.5*x1+40
# ax.plot_surface(x1, y1, z1,cmap='viridis', edgecolor='none')
# y2 = np.linspace(0,20,80)
# y2,z1 = np.meshgrid(y2,z1)
# x2 = 60-y2
# ax.plot_surface(x2, y2, z1,cmap='viridis', edgecolor='none')
# z3 = 60
# x3 = np.linspace(0,40,100)
# x3, z3 = np.meshgrid(x3,z3)
# y3 = 40-0.5*x3
# ax.plot_surface(x3, y3, z3,cmap='viridis', edgecolor='none')

# att1 = pd.DataFrame(data= placex)
# att2 = pd.DataFrame(data= placey)
# att3 = pd.DataFrame(data= placez)
# att4 = pd.DataFrame(data= placexs)
# att5 = pd.DataFrame(data= placeys)
# att6 = pd.DataFrame(data= placezs)
# att7 = pd.DataFrame(data= tmpx)
# att8 = pd.DataFrame(data= tmpy)
# att9 = pd.DataFrame(data= tmpz)

# att1.to_csv('D:\\fig:x1.csv')
# att2.to_csv('D:\\fig:y1.csv')
# att3.to_csv('D:\\fig:z1.csv')
# att4.to_csv('D:\\fig:x2.csv')
# att5.to_csv('D:\\fig:y2.csv')
# att6.to_csv('D:\\fig:z2.csv')
# att7.to_csv('D:\\fig:x3.csv')
# att8.to_csv('D:\\fig:y3.csv')
# att9.to_csv('D:\\fig:z3.csv')

ax.scatter3D(placex,placey,placez,c='blue',s = 0.1)
#ax.scatter3D(placexs,placeys,placezs,c='blue',s = 0.1)
#ax.scatter3D(tmpx,tmpy,tmpz,c='blue',s = 0.1)
plt.show()