# import sys
# import json
import time
import pandas
from algo_struct.algo import my_interpolate
from algo_struct.myStruct import ObjectInfo
from dynamics_simulation import motionSim
from navigation import gpsAndUtm
# from out_interface import fileDeal
import numpy as np
import queue
import matplotlib.pyplot as plt

# import udpSingleBoard
import os
os.getcwd()

# -降低功能模块的耦合度 在变换了平台之后 只需要修改工控机
# -将路径规划等复杂算法、程序块转移到python脚本 工控机专注于平台的驱动路径规划等不需要改动
# -使用gitee管理

print("hello world!")

# 路径生成
class PathGen:
	def __init__(self):
		self.q = queue.Queue()# 消息队列 线程间通信用

		end_h = 1000# 终止时间
		set_speed = 4 # 期望定常跟踪速度
		init_x = np.array([0, 0, 0, 0, 0, 0])# 初始无人艇位置
		self.init_lon = 120.30754612708418 # 初始经度
		self.init_lat = 31.49450233916176 # 初始纬度
		# ---插值获得路径 也可用其他方法获得路径
		# ---插值的X 必须是单调增加的
		path_observed_x = np.array([20, 200, 400, 600]) # 插值点纵轴坐标
		path_observed_y = np.array([20, 200, 200, 100]) # 插值点横轴坐标
		# 样条插值算法
		path_observed_x, path_observed_y = my_interpolate(path_observed_x, path_observed_y, method='cubic')
		self.path = np.zeros(shape=[len(path_observed_x), 3])
		for i in range(len(path_observed_x)):
			self.path[i][0] = path_observed_x[i]
			self.path[i][1] = path_observed_y[i]
		# 原始路径点 n*[北 东 地] 本a地北东地坐标系

		# 生成障碍物位置 这里是从UDP接收
		self.objs = []
		# 测试用障碍物 位置3 速度1 大地运动方向角1 长度1 宽度1 高度1 危险半径1 类型1 n*10矩阵
		obj = ObjectInfo()
		obj.x = np.array([600, 100, 0, 0, 0, 0])
		obj.nu = np.array([1, 1, 0, 0, 0, 0])
		obj.lhw = np.array([20, 20, 10])
		self.objs.append(obj)
		# 设置 最大仿真时间 最大跟踪速度 初始机器人位置
		self.boat_set = [end_h, set_speed, init_x]
		self.mode = 1 #设置路径跟踪模式

	def plot_motion_slot(self, point):
		# 完成循环仿真 开始绘图
		# 在最后进行整体经纬度变换，取北东地坐标系
		'''

		:param point: list[ndarray]
		:return: x-list, y-list
		'''
		x = []
		y = []
		z = []
		phi = []
		theta = []
		psi = []
		# print('len of points:', len(points))
		for var in point:
			# print('var is :', var)
			# 获取x坐标
			x.append(var[0])
			# 获取y坐标
			y.append(var[1])
			z.append(var[2])
			phi.append(var[3] * 180 / np.pi)
			# 获取航向角
			theta.append(var[4] * 180 / np.pi)
			psi.append(var[5] * 180 / np.pi)
		# print('x is :', x)
		# print('y is :', y)
		# print('x,y len is :', len(x), len(y))
		return x, y, z, phi, theta, psi

	def plot_path(self, some_pa):
		'''
		转换数据格式
		:param some_pa:
		:return:
		'''
		x = []
		y = []
		for var in some_pa:
			x.append(var[0])
			y.append(var[1])
		return x, y

	def plot_circle(self, x, y, r):
		'''
		圆形
		:param x: 圆心坐标x
		:param y: 圆心坐标y
		:param r: 圆半径
		:return: 圆一圈的点
		'''
		a_x = np.arange(0, 2 * np.pi, 0.01)
		a = x + r * np.cos(a_x)
		b = y + r * np.sin(a_x)
		return a, b

	def path_generator(self):
		start_t = time.time()
		# ----------------------------------- BLOCK MOTION SIMULATION START -----------------------------------
		# boat运动仿真类输入参数
		my_mmg = motionSim.BoatMotionSim(q=self.q, path=self.path, objs=self.objs, boat_set=self.boat_set,
		                                 mode=self.mode)

		# 启动线程 开始run
		my_mmg.start()
		while True:
			if not self.q.empty():
				# 消息队列中获取到了值 跨线程传递仿真结果

				break
		# 消息队列先进先出
		points = self.q.get()
		# 等待线程退出
		my_mmg.join()
		# 计算代码所用时间
		end_time = time.time()
		print('MOTION SIMULATION TIME IS:', end_time - start_t)

		points_wgs84 = gpsAndUtm.get_list_wgs84(init_gps84=[self.init_lon, self.init_lat], points_xyz=points)
		# -------------------------------------- PLOT START----------------------------------------
		# 绘制第一张图 本地坐标系
		rx, ry, rz, phi, theta, psi = self.plot_motion_slot(points)
		plt.subplot(3, 1, 1)
		# 绘图坐标系和仿真坐标系不一致 改成一致
		plt.plot(ry, rx, label="actual motion path")
		plt.xlabel('east/m')
		plt.ylabel('north/m')
		if self.mode == 1:
			# 路径跟踪
			# print('points is ', points)
			sx, sy = self.plot_path(self.path)
			plt.plot(sy, sx, label="desire path")
			plt.legend()
		elif self.mode == 2:
			# 避障
			for obj in self.objs:
				# 开始避障
				a, b = self.plot_circle(obj.x[0], obj.x[1], obj.danger_r)
				plt.plot(b, a, linestyle='-')
				# 实际障碍半径
				a, b = self.plot_circle(obj.x[0], obj.x[1], 0.3 * obj.danger_r)
				plt.plot(b, a, linestyle='-')

		# WGS84坐标
		# plt.subplot(4, 1, 2)
		# plt.plot(points_wgs84[:, 0], points_wgs84[:, 1], label="WGS84 motion path")
		# plt.xlabel('lon/deg')
		# plt.ylabel('lat/deg')
		# plt.legend()

		plt.subplot(3, 1, 2)
		m_time = 0.5 * np.linspace(0, len(rz) - 1, len(rz))
		plt.plot(m_time, rz, label="z motion")
		plt.xlabel('t/s')
		plt.ylabel('z/m')
		plt.legend()
		plt.subplot(3, 1, 3)
		# m_time = np.linspace(0,len(rz)-1,len(rz))
		plt.plot(m_time, phi, label="phi motion")
		plt.plot(m_time, theta, label="theta motion")
		plt.xlabel('t/s')
		plt.ylabel('angle/deg')
		plt.legend()
		plt.show()

		# 写入数据保存
		df = pandas.DataFrame({
			'time':m_time,
			'z/m':rz,
			'phi':phi,
			'theta':theta
		})
		# 把仿真数据保存到CSV
		df.to_csv('df.csv')

		# ------------------------------------- PLOT END ------------------------------------------
		return points_wgs84


# ----------------------------------- BLOCK MOTION SIMULATION END -----------------------------------


# ----------------------------------- TCP CLIENT START ---------------------------------

# test motion sim
if __name__ == '__main__':
	my = PathGen()
	points_gps = my.path_generator()

# ----------------------------------- TCP CLIENT END -----------------------------------