from tqdm import tqdm
import concurrent.futures
import math
import os
from csv import reader
import pandas as pd
import position_transfer
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt

# 第一个文件的第一个有效帧，不用这个
# < longitude > 124.671000 < / longitude >
# < latitude > 39.902700 < / latitude >
# < altitude > 44.743948 < / altitude >

#第一行
start_long=124.517535
start_lati=39.947593
start_altitude=0


def cal_objective_position(x,y,z,tarRng,tarAzi,tarEli):
    tar_x = x + tarRng * math.sin(tarAzi) * math.cos(tarEli)
    tar_y = y + tarRng * math.cos(tarAzi) * math.cos(tarEli)
    tar_z = z + tarRng* math.sin(tarEli)
    return tar_x,tar_y,tar_z


def process(file_path,file_name):
    filename = file_name.split(".")[0]
    df = pd.read_excel(file_path)
    df['tar_x']=''
    df['tar_y']=''
    df['tar_z']=''

    # df.to_csv('example.csv', index=False)
    for index,row in df.iterrows():
        longitude=row['longitude']
        latitude=row['latitude']
        altitude=row['altitude']
        tarRng=row['tarRng']
        tarEli=row['tarEli']
        tarAzi=row['tarAzi']
        result=position_transfer.gps_to_xyz(latitude,longitude,altitude,start_lati,start_long,start_altitude)
        x,y,z=cal_objective_position(result[0],result[1],result[2],tarRng,tarAzi,tarEli)

    #     print(x,y,z)
    #     x_arr.append(x)
    #     y_arr.append(y)
    #     z_arr.append(z)
    # return x_arr,y_arr,z_arr
        df.loc[index, 'tar_x'] = x
        df.loc[index, 'tar_y'] = y
        df.loc[index, 'tar_z'] = z

    df.to_csv('E:/replay/1对1/data/1/'+filename+'.csv', index=False)
    return filename + 'finish'

def processFiles():
    folder_path = "E:/replay/1对1/original_data/5"  # 替换为你的文件夹路径
    # folder_path = "E:/replay/demo"
    # # 获取文件夹中的所有文件
    file_list = os.listdir(folder_path)
    results = []
    count = 1
    with concurrent.futures.ProcessPoolExecutor(max_workers=5) as executor:
        for file_name in tqdm(file_list, desc="Processing files"):
            if file_name.endswith(".xlsx"):
                file_path = os.path.join(folder_path, file_name)
                # 解析XML文件
                result = executor.submit(process, file_path, file_name)
                results.append(result)
        for future in concurrent.futures.as_completed(results):
            print(count)
            print(future.result())
            count += 1

def cal_features(file_path,file_name):
    filename = file_name.split(".")[0]
    df = pd.read_csv(file_path)
    df['position_change'] = ''
    df['v'] = ''
    df['v_change'] = ''
    df['r']=''
    df['eli_change']=''
    df['h_change']=''
    # df.drop('e_change',axis=1,inplace=True)
    for index, row in df.iterrows():
        if index > 0:
            x = row['tar_x']
            y = row['tar_y']
            z = row['tar_z']
            e=row['tarEli']
            x_pre=df.loc[index-1, 'tar_x']
            y_pre=df.loc[index-1, 'tar_y']
            z_pre=df.loc[index-1, 'tar_z']
            e_pre=df.loc[index-1, 'tarEli']
            position_change=math.sqrt((x_pre-x)**2+(y_pre-y)**2+(z_pre-z)**2)
            h_change=z-z_pre
            e_change=e-e_pre
            speed=position_change/0.1
            df.loc[index, 'position_change'] = position_change
            df.loc[index,'v']=speed
            df.loc[index, 'h_change'] = h_change
            df.loc[index, 'eli_change'] = e_change
            speed_change=0
            if index!=1:
                speed_change=speed-df.loc[index-1,'v']
            df.loc[index,'v_change']=speed_change
            if x-x_pre!=0:
                r=math.atan((y-y_pre)/(x-x_pre))/0.1
            else:
                r=0
            df.loc[index, 'r'] = r

    df.to_csv('E:/replay/1对1/data/1/'+filename+'.csv', index=False)
    return filename + 'finish'


def cal_features_for_files():
    folder_path = "E:/replay/1对1/data/5"  # 替换为你的文件夹路径
    # folder_path = "E:/replay/demo"
    # # 获取文件夹中的所有文件
    file_list = os.listdir(folder_path)
    results = []
    count = 1
    with concurrent.futures.ProcessPoolExecutor(max_workers=5) as executor:
        for file_name in tqdm(file_list, desc="Processing files"):
            if file_name.endswith(".csv"):
                file_path = os.path.join(folder_path, file_name)
                # 解析XML文件
                result = executor.submit(cal_features, file_path, file_name)
                results.append(result)
        for future in concurrent.futures.as_completed(results):
            print(count)
            print(future.result())
            count += 1


def draw():
    # 设置图例字号
    folder_path = "E:/replay/1对1/process_data/5"  # 替换为你的文件夹路径
    # folder_path = "E:/replay/demo"
    # # 获取文件夹中的所有文件
    file_list = os.listdir(folder_path)
    mpl.rcParams['legend.fontsize'] = 10
    # 方式2：设置三维图形模式
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    count=0
    for file_name in tqdm(file_list, desc="Processing files"):
        if count>10:
            break
        if file_name.endswith(".csv"):
            file_path = os.path.join(folder_path, file_name)
            df = pd.read_csv(file_path)
            count+=1
            x_arr = []
            y_arr = []
            z_arr = []
            # df.to_csv('example.csv', index=False)
            for index, row in df.iterrows():
                if index>1:
                    x = row['tar_x']
                    y = row['tar_y']
                    z = row['tar_z']
                    x_arr.append(x)
                    y_arr.append(y)
                    z_arr.append(z)
                # 绘制图形
            ax.scatter(x_arr, y_arr, z_arr, s=2)


def change_colum(file_path):
    df = pd.read_csv(file_path)
    df = df[['number', 'longitude', 'latitude', 'altitude', 'yaw', 'pitch', 'roll',
             'tarRng', 'tarAzi', 'tarEli', 'tar_x', 'tar_y', 'tar_z', 'position_change', 'v',
             'v_change', 'r', 'eli_change', 'h_change']]
    df.to_csv(file_path, index=False)
    return 'finish'

def change_colum_for_files():
    folder_path = "E:/replay/1对1/data/1"  # 替换为你的文件夹路径
    # folder_path = "E:/replay/demo"
    # # 获取文件夹中的所有文件
    file_list = os.listdir(folder_path)
    results = []
    count = 1
    with concurrent.futures.ProcessPoolExecutor(max_workers=5) as executor:
        for file_name in tqdm(file_list, desc="Processing files"):
            if file_name.endswith(".csv"):
                file_path = os.path.join(folder_path, file_name)
                # 解析XML文件
                result = executor.submit(change_colum, file_path)
                results.append(result)
        for future in concurrent.futures.as_completed(results):
            print(count)
            print(future.result())
            count += 1


if __name__ == '__main__':
    folder_path = "E:/replay/1对1/data/2/"
    file_path=folder_path + "default - 20230601-134029.csv"

    # df = pd.read_csv(file_path)
    # df = df[['number', 'longitude', 'latitude', 'altitude', 'yaw', 'pitch', 'roll',
    #                            'tarRng', 'tarAzi', 'tarEli','tar_x','tar_y','tar_z','position_change','v',
    #                         'v_change','r','eli_change','h_change']]
    # df.to_csv(file_path, index=False)
    # process(file_path1,file_name1)
    # cal_features(file_path, file_name)
    # processFiles()
    # cal_features_for_files()
    # change_colum_for_files()

    draw()
    plt.show()

    # 显示图形
    # plt.show()
