import datetime
import os
import sys
import numpy as np
from PyQt5.QtCore import pyqtSignal, QObject
from PyQt5.QtWidgets import QMessageBox, QApplication

from UI import AutoTractorWindow
from autodrive import globalvars as glv
from autodrive.VisualEle import Field, QFieldView, WorkingPath
from autodrive.vehicles import Tractor, Trailer
from autodrive.controllers import SmartDriver, Radar
from autodrive.fun import deg2rand, rand2deg
from autodrive.navigators import XN422
from autodrive.ATCore import RadioStation


class AutoTractor(QObject):
    RadarSignal = pyqtSignal(list)

    def __init__(self):
        super().__init__()
        glv.init()
        # #############重要更换信息
        ########################
        glv.set_value('COM_NAME', 'COM5')  # 设置端口号
        glv.set_value('LENGTH_TRACTOR_HALF', 0.0)  # 设置拖拉机定位点变换距离
        glv.set_value('L0', 300.0)  # 设置拖拉机定位点到挂接点距离，970和400
        glv.set_value('INIT_HEADING', 90.0)  # 设置拖拉机和拖车初始航向角（根据实际导航信息更换）
        ########################
        # 创建主程序
        self.app = QApplication(sys.argv)
        # 创建主窗口
        self.mainWindow = AutoTractorWindow()
        # 加载田野文件
        self.fields = Field()
        # 作业路径
        self.working_path = WorkingPath()
        # 路径初始化
        self.current_path_section = 0
        self.path_points_number = 0

        # 加载车辆模型
        self.tractor = Tractor()
        self.trailer = Trailer()

        # 加载神经网络控制器
        self.smart_drive = SmartDriver()

        # 创建雷达接口
        self.radar = Radar(1000, 1, 400)

        # 接收XN422数据
        self.xn422 = XN422()

        # 发送行驶命令
        self.radio_station = RadioStation()
        self.radio_station.start()

        # 设置视图
        self.field_view = QFieldView()
        # 自动驾驶标志位初始化为false
        self.isAutoDriving = False

        # 链接信号与槽
        # 加载田野，路径，车辆，控制器
        self.mainWindow.load_field_signal.connect(self.load_field)
        self.mainWindow.load_path_signal.connect(self.load_path)
        self.mainWindow.load_tractor_signal.connect(self.load_tractor)
        self.mainWindow.load_trailer_signal.connect(self.load_trailer)
        self.mainWindow.load_brain_signal.connect(self.load_brain)
        # 开启自动驾驶
        self.mainWindow.auto_driving.connect(self.autopilot)
        # 导航
        self.tractor.Moved_Signal.connect(self.on_tractor_move)
        self.RadarSignal.connect(self.field_view.OnRadarUpdate)
        self.tractor.tractor_pos_info.connect(self.field_view.OnTractorUpdate)
        self.trailer.trailer_pos_info.connect(self.field_view.OnTrailerUpdate)
        self.trailer.trailer_pos_info.connect(self.tractor.get_trailer_pos)
        self.smart_drive.smart_driver_error.connect(self.on_smart_driver_error)

        # 车辆位姿更细及命令传输
        self.radio_station.getMessageSignal.connect(self.xn422.refresh)
        self.xn422.position_update.connect(self.tractor.get_tractor_pos)

        # GNSS面板显示信息
        self.xn422.GNSS_information.connect(self.field_view.GNSS_information)

        # 控制命令发出
        self.tractor.Drive_CMD.connect(self.radio_station.send_message)
        self.mainWindow.setCentralWidget(self.field_view)  # 没有这句显示不了
        self.mainWindow.show()
        sys.exit(self.app.exec())

    def load_field(self, field_file):
        self.fields.load(field_file)
        self.field_view.load_field(self.fields)

    def load_path(self, path_file):
        self.working_path.load(path_file)
        self.field_view.load_working_path(self.working_path)
        self.path_points_number = len(self.working_path.points)

    def load_tractor(self, tractor_file):
        self.tractor.load(tractor_file)
        self.field_view.load_tractor(self.tractor)

    def load_trailer(self, trailer_file):
        self.trailer.load(trailer_file)
        self.field_view.load_trailer(self.trailer)

    def load_brain(self, brain_file):
        self.smart_drive.load(brain_file)

    def on_smart_driver_error(self):
        self.isAutoDriving = False

    def autopilot(self, status):

        if len(self.working_path.points) == 0:
            dlg = QMessageBox(QMessageBox.Critical, '错误', '请载入作业路径', QMessageBox.Ok)
            dlg.exec()
            return
        if status:
            tractor_position = np.array([self.tractor.base_point[0], self.tractor.base_point[1]])
            path_start_point = np.array([self.working_path.points[0].x(), self.working_path.points[0].y()])
            distance = np.linalg.norm(tractor_position - path_start_point)
            if distance > 900:
                # 车辆起始位置太远重新放置车辆起始位置
                dlg = QMessageBox(QMessageBox.Critical, '错误', '请将车辆移至起始点', QMessageBox.Ok)
                dlg.exec()
                return
            dlg = QMessageBox(QMessageBox.Warning, '提示', '点击确定将开始自动驾驶', QMessageBox.Ok | QMessageBox.Cancel)
            if dlg.exec() == QMessageBox.Cancel:
                return
            # 启动自动驾驶
            self.isAutoDriving = True
            # 启动拖拉机
            self.tractor.forward()
        else:
            self.isAutoDriving = False
            self.tractor.stop()

    def on_tractor_move(self):
        if self.isAutoDriving:
            if self.current_path_section < self.path_points_number - 1:
                start_pnt = [self.working_path.points[self.current_path_section].x(),
                             self.working_path.points[self.current_path_section].y()]
                end_pnt = [self.working_path.points[self.current_path_section + 1].x(),
                           self.working_path.points[self.current_path_section + 1].y()]
                if np.linalg.norm(np.array(end_pnt) - np.array(self.tractor.base_point)) < 300:
                    self.current_path_section += 1
                else:
                    self.radar.set_path(start_pnt[0], start_pnt[1], end_pnt[0], end_pnt[1])
                    radar_data = self.radar.scan(
                        [self.tractor.base_point[0], self.tractor.base_point[1], self.tractor.direction])
                    self.RadarSignal.emit(radar_data)
                    self.tractor.steering = self.smart_drive.prediction(radar_data)
            else:
                self.tractor.drive_cmd = 3  # 到达终点停车
                self.tractor.stop()
                self.isAutoDriving = False
                dlg = QMessageBox(QMessageBox.Critical, '结束', '已经到达终点', QMessageBox.Ok)
                dlg.exec()
                return


if __name__ == '__main__':
    AutoTractor()