import math

from PyQt5.QtWidgets import QWidget, QFrame, QLabel, QLineEdit, QPushButton, QComboBox, QCheckBox
from PyQt5.QtGui import QPixmap, QPainter, QResizeEvent, QCloseEvent, QPaintEvent, QFont
from PyQt5.QtCore import QSize, QTimer, Qt
from RobotData import Robot
import dijkstra.Map as mp


class ControlFrame(QFrame):

    def __init__(self, UI_data):
        QFrame.__init__(self)
        [self.name_, self.nodes_] = UI_data
        self.robot_ = Robot(self.name_)
        self.setGeometry(0, 0, 500, 500)
        self.InitUI()
        self.setFrameShape(self.StyledPanel)
        self.timer_ = QTimer()
        self.timer_.timeout.connect(self.Update)
        self.timer_.start(100)

    def InitUI(self):

        self.basestatic = QLabel(self)
        self.basestatic.setText(self.name_)
        self.basestatic.setGeometry(20, 20, 80, 30)

        self.begstatic = QLabel(self)
        self.begstatic.setText("  起点")
        self.begstatic.setGeometry(100, 5, 50, 30)
        self.begQc = QComboBox(self)
        self.begQc.setGeometry(160, 5, 80, 30)

        self.endstatic = QLabel(self)
        self.endstatic.setText("  终点")
        self.endstatic.setGeometry(100, 50, 50, 30)
        self.endQc = QComboBox(self)
        self.endQc.setGeometry(160, 50, 80, 30)

        self.btnSend = QPushButton(self)
        self.btnSend.setGeometry(150, 100, 100, 40)
        self.btnSend.setText("  启动导航")
        self.btnSend.clicked.connect(self.btnSendClick)

        self.btnCancel = QPushButton(self)
        self.btnCancel.setGeometry(150, 150, 100, 40)
        self.btnCancel.setText("  取消导航")
        self.btnCancel.clicked.connect(self.btnCancelClick)

        self.begQc.addItems(self.nodes_)
        self.endQc.addItems(self.nodes_)
        self.endQc.activated.connect(self.endQCChanged)

        self.posestatic = QLabel(self)
        self.posestatic.setGeometry(260, 5, 100, 90)

        self.LedLabel = QLabel(self)
        self.LedLabel.setGeometry(80, 120, 50, 50)

        # 发送轴距
        self.wheelbasestatic = QLabel(self)
        self.wheelbasestatic.setText("轴距:")
        self.wheelbasestatic.setGeometry(260, 100, 40, 30)
        self.WheelBaseEdit = QLineEdit(self)
        self.WheelBaseEdit.setText("3.0")
        self.WheelBaseEdit.setGeometry(300, 100, 50, 30)

        self.btnModelCheck = QCheckBox("双车模式", self)
        self.btnModelCheck.setGeometry(260, 150, 100, 40)
        self.btnModelCheck.stateChanged.connect(self.MainAgvchangecb)

        self.btnPark = QPushButton(self)
        self.btnPark.setGeometry(150, 200, 100, 40)
        self.btnPark.setText("  存车")
        self.btnPark.clicked.connect(self.btnParkClick)

        self.btnPick = QPushButton(self)
        self.btnPick.setGeometry(150, 250, 100, 40)
        self.btnPick.setText("  取车")
        self.btnPick.clicked.connect(self.btnPickClick)


    def Update(self):
        if self.robot_.timedPose_.timeout() == False:
            x = self.robot_.timedPose_.statu.x
            y = self.robot_.timedPose_.statu.y
            yaw = self.robot_.timedPose_.statu.theta
            self.posestatic.setText("x : %.3f\ny : %.3f\nΘ : %.2f" % (x, y, yaw * 180 / math.pi))

            '''djks_map=mp.DijikstraMap()
            [label,pt]=djks_map.findNeastNode([x,y])
            if self.nodes_.index(label)>=0:
                self.begQc.setCurrentText(label)'''

        statu = "min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px;  border:1px solid black;background:green"
        if self.robot_.Connected() == False:
            statu = "min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px; border:1px solid black;background:gray"
            # self.endQc.setEnabled(False)
        elif self.robot_.IsNavigating() == True:
            statu = "min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px; border:1px solid black;background:yellow"
            self.endQc.setEnabled(False)
            self.btnModelCheck.setEnabled(False)
        else:
            self.endQc.setEnabled(True)
            self.btnModelCheck.setEnabled(True)
        self.LedLabel.setStyleSheet(statu)

        self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
        self.WheelBaseEdit.setVisible(self.robot_.IsMainAgv())
        self.btnModelCheck.setVisible(self.robot_.IsMainAgv())

    def MainAgvchangecb(self):
        if self.btnModelCheck.checkState() == Qt.Checked:
            leng = float(self.WheelBaseEdit.text())
            self.robot_.SwitchMoveMode(2, leng)
        if self.btnModelCheck.checkState() == Qt.Unchecked:
            self.robot_.SwitchMoveMode(1, 0)

    def endQCChanged(self):
        id1 = self.begQc.currentText()
        id2 = self.endQc.currentText()
        self.robot_.GeneratePath(id1, id2)

    def btnSendClick(self):
        self.robot_.wheelbase_ = 3
        self.robot_.ExecNavtask()

    def btnCancelClick(self):
        self.robot_.CancelNavTask()

    def btnParkClick(self):
        id1 = self.begQc.currentText()
        id2 = self.endQc.currentText()
        self.robot_.ParkTask(begID=id1, targetID=id2)

    def btnPickClick(self):
        id1 = self.begQc.currentText()
        id2 = self.endQc.currentText()
        self.robot_.PickTask(begID=id1, targetID=id2)