import numpy as np
import pandas as pd
from readpos import readpos
import math
from geopy.distance import geodesic
import matplotlib.pyplot as plt

plt.rcParams['font.sans-serif']=['SimHei']
plt.rcParams['axes.unicode_minus']=False



LG69T_path = "./llh.csv"
TC_Nav_path = "/Users/wakanda_shaw/Desktop/TC_Nav_Zjut/TC_NAV_Zjut_post/data/630_3/llh.pos"

D2R = math.pi/180

# 从.pos文件得到python lsit 格式数据
llh = readpos(TC_Nav_path)

LG69T_data = pd.read_csv(LG69T_path,names=['tow','lat','lon'],dtype=np.float64)

# list 转换到 dataframe
TC_Nav_data = pd.DataFrame(llh,columns=['tow','lat','lon'])

x=[]
y=[]
for i in range(len(TC_Nav_data)):
    tow = TC_Nav_data.iloc[i,0]
    if(tow==367036):
        break
    TC_lat = TC_Nav_data.iloc[i,1].round(10)
    TC_lon = TC_Nav_data.iloc[i,2].round(10)

    if tow==17 or tow==89:
        continue
    try:
        LG_lat = LG69T_data.loc[(LG69T_data['tow']-18)==tow,['lat']]
        LG_lat = LG_lat.iloc[0,0].round(10)
        LG_lon = LG69T_data.loc[(LG69T_data['tow']-18)==tow,['lon']]
        LG_lon = LG_lon.iloc[0,0].round(10)
    except:
        print(i,"pass")
        continue

    distance = geodesic((TC_lat,TC_lon),(LG_lat,LG_lon)).m
    print(i,"距离：{:.3f}m".format(distance))

    x.append(tow)
    y.append(distance)

plt.plot(x,y,label='TC_Nav#C')
plt.legend(loc = 'upper right')
plt.grid()  # 生成网格
plt.xlabel("time(s)")
plt.ylabel("distance(m)")
# plt.title(u'半遮挡情况下误差(m)')
plt.show()