import datetime
import math
import socket

import numpy as np
from loguru import logger

clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
clientSocket.connect(('192.168.43.100', 4002))

# # 发
# sendData = input('>>')
# clientSocket.send(sendData.encode('gbk'))
# 收
# jishi=0
# while jishi<= 30:

x = 0
y = 0
yaw = 0
x_car = 0
y_car = 0
heading = 0
east = 0
north = 0
beidou = 0


def zuo_biao():
    global yaw, x_car, y_car, east, north, beidou, heading
    while True:
        recvData = clientSocket.recv(1024)
        tcpdata = recvData.decode('gbk')
        # print(tcpdata)
        beidou = tcpdata.split(',')
        # print(beidou)
        # print(len(beidou))
        # KSXT = beidou[0]
        # if KSXT=='$KSXT':
        KSXT = beidou[0]  # 帧头
        time0 = beidou[1]  # 时间
        lon = beidou[2]  # 经度
        lat = beidou[3]  # 纬度
        height = beidou[4]  # 海拔

        heading = beidou[5]  # 方位角
        heading = float(heading)
        heading = heading + 180
        if heading > 360:
            heading = heading - 360
        if 0 < heading <= 90:
            yaw = math.radians(90 - float(heading))
            yaw = round(yaw, 3)

        else:
            yaw = math.radians(450 - float(heading))
            yaw = round(yaw, 3)
        print("yaw", yaw)

        # pitch = beidou[6]  # 俯仰角
        # tracktrue = beidou[7]  # 速度角
        # vel = beidou[8]  # 水平速度
        # roll = beidou[9]  # 横滚  空
        # pos_qual = beidou[10]  # GNSS定位质量指示符0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
        # heading_qual = beidou[11]  # GNSS定向质量指示符 0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
        # solnsvsh = beidou[12]  # 前天线使用卫星数 前天线当前参与解算的卫星数量
        # solnsvsb = beidou[13]  # 后天线使用卫星数 前天线当前参与解算的卫星数量
        if abs(x_car-float(east))>10 or abs(y_car-float(north))>10:
            x_car=float(east)
            y_car=float(north)
        east = beidou[14]  # 东向位置坐标：以基站为原点的地理坐标系下的东向位置，单位：米，小数点后 3 位
        x_car = np.cos(yaw) * 2.3 + float(east)
        x_car = round(x_car, 3)
        # print('x',x_car)
        north = beidou[15]  # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
        y_car = -np.sin(yaw) * 2.3 + float(north)
        y_car = round(y_car, 3)
        shijian = datetime.datetime.now()

    # logger.add(
    # 	'x,y记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
    # logger.debug('北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(east), str(north), str(x_car),
    # 			 str(y_car), str(beidou[5]), str(heading))
    # up = beidou[16]  # 天向位置坐标：以基站为原点的地理坐标系下的天顶向位置，单位：米，小数点后 3 位
    # eastvel = beidou[17]  # 东向速度：地理坐标系下的东向速度，小数点后 3位，单位：Km/h(如无为空)
    # northvel = beidou[18]  # 北向速度：地理坐标系下的北向速度，小数点后 3位，单位：Km/h(如无为空)
    # uphvel = beidou[19]  # 天向速度：地理坐标系下的天顶向速度，小数点后3 位，单位：Km/h(如无为空)
    # unknown = beidou[20]
    # check = beidou[21]  # 异或校验（十六进制字符串，从帧头开始校验）
    # east = float(east)
    # north = float(north)

# print('接收时间为:',datetime.datetime.now())
# print('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s; ' % ( heading, east, north))
# print('\n')

# shijian=datetime.datetime.now()

# file = open('1.txt', 'a')
# file.write(str('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s; ' % (heading, east, north)))
# file.write('\n')
# jishi=jishi+1
# time.sleep(0.05)

# else:
#     print('未接收到定位消息，重新连接')
#     clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#     clientSocket.connect(('192.168.43.100', 4002))
#     # clientSocket.close()

# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
# if 0.2>=d >= 0.1:
# 	print('大于', d)
# 	print('\n')
# 	file = open('1.txt', 'a')
# 	file.write(str('方位角：%s;  东向位置坐标：%s; 北向位置坐标：%s;与上一点的距离为%s ' % (heading, x, y,d)))
# 	file.write('\n')
# 	x = east
# 	y = north
# 	print(x, y)
# else:
# 	print('不在范围', d)
# 	print('\n')
# 	if x==0 and y==0:
# 		x = east
# 		y = north
# time.sleep(0.05)
# zuo_biao()