# coding:utf-8

import sys
sys.path.append("..")
from pypibot import log
import pypibot

import params
import dataholder
from dataholder import MessageID
from transport import Transport
from PyQt5.QtWidgets import QApplication, QDialog
from PyQt5.QtCore import QObject,pyqtSignal
import pb
import threading

port = "COM9"

pypibot.assistant.enableGlobalExcept()
# log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i")


class MainDialog(QDialog):
    encoder_signal = pyqtSignal(list)
    imu_signal = pyqtSignal(list)
    pid_debug_signal = pyqtSignal(list)

    def __init__(self, parent=None):
        super(QDialog, self).__init__(parent)
        self.ui = pb.Ui_pb()
        self.ui.setupUi(self)
        self.model_type_list = {"2wd-diff": dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF,
                                "4wd-diff": dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF,
                                "3wd-omni": dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI,
                                "4wd-omni": dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI,
                                "4wd-mecanum": dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM}

        self.model_value_list = {0: dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF,
                                1: dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF,
                                2: dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI,
                                3: dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI,
                                3: dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM}
        
        self.model_index_list = {dataholder.RobotModelType.MODEL_TYPE_2WD_DIFF: 0,
                                 dataholder.RobotModelType.MODEL_TYPE_4WD_DIFF: 1,
                                 dataholder.RobotModelType.MODEL_TYPE_3WD_OMNI: 2,
                                 dataholder.RobotModelType.MODEL_TYPE_4WD_OMNI: 3,
                                 dataholder.RobotModelType.MODEL_TYPE_4WD_MECANUM: 4}

        self.imu_type_list = {"gy65": 0, "gy85": 1, "gy87": 2}
        self.imu_value_list = {49: "gy65", 69: "gy85", 71: "gy87"}
        self.imu_index_list = {0: 49, 1: 69, 2: 71}

        self.init_ui()
        self.mboard = None

        self.init_dev()

        self.encoder_signal.connect(self.update_encoder)
        self.imu_signal.connect(self.update_imu)
        self.pid_debug_signal.connect(self.update_pid_debug)

        self._KeepRunning = True
        self.encoder_thread = threading.Thread(target=self._read_encoder)
        self.encoder_thread.start()

    def closeEvent(self, event):
        self._KeepRunning = False

    def init_ui(self):
        for model_type in self.model_type_list.keys():
            self.ui.combox_model.addItem(model_type)

        for imu_type in self.imu_type_list.keys():
            self.ui.combox_imu_type.addItem(imu_type)

        self.ui.slider_wheel_diameter.setMinimum(10)
        self.ui.slider_wheel_diameter.setMaximum(500)

        self.ui.slider_wheel_track.setMinimum(50)
        self.ui.slider_wheel_track.setMaximum(1000)

        self.ui.slider_encoder.setMinimum(1)
        self.ui.slider_encoder.setMaximum(500)

        self.ui.slider_motor_ratio.setMinimum(1)
        self.ui.slider_motor_ratio.setMaximum(1000)

        self.ui.slider_pid_interval.setMinimum(1)
        self.ui.slider_pid_interval.setMaximum(80)
        self.ui.slider_kp.setMinimum(0)
        self.ui.slider_kp.setMaximum(10000)
        self.ui.slider_ki.setMinimum(0)
        self.ui.slider_ki.setMaximum(32000)
        self.ui.slider_kd.setMinimum(0)
        self.ui.slider_kd.setMaximum(1000)
        self.ui.slider_ko.setMinimum(0)
        self.ui.slider_ko.setMaximum(1000)

        self.ui.slider_cmd_lasttime.setMinimum(0)
        self.ui.slider_cmd_lasttime.setMaximum(1000)
        self.ui.slider_vx_max.setMinimum(0)
        self.ui.slider_vx_max.setMaximum(500)
        self.ui.slider_vy_max.setMinimum(0)
        self.ui.slider_vy_max.setMaximum(500)
        self.ui.slider_va_max.setMinimum(0)
        self.ui.slider_va_max.setMaximum(2000)

        self.ui.comboBox_support_model.setVisible(False)
        self.ui.pushButton_load.setVisible(False)

        self.ui.pushButton_read.clicked.connect(self.read_params)
        self.ui.pushButton_set.clicked.connect(self.write_params)
        self.ui.pushButton_start.clicked.connect(self.start_motor)
        self.ui.pushButton_stop.clicked.connect(self.stop_motor)

        self.ui.pushButton_start_2.clicked.connect(self.start_control)
        self.ui.pushButton_stop_2.clicked.connect(self.stop_control)

        self.ui.slider_set_pwm1.setMinimum(-5000)
        self.ui.slider_set_pwm1.setMaximum(5000)
        self.ui.slider_set_pwm2.setMinimum(-5000)
        self.ui.slider_set_pwm2.setMaximum(5000)
        self.ui.slider_set_pwm3.setMinimum(-5000)
        self.ui.slider_set_pwm3.setMaximum(5000)
        self.ui.slider_set_pwm4.setMinimum(-5000)
        self.ui.slider_set_pwm4.setMaximum(5000)
        self.ui.tabWidget.setTabText(0, '1.参数配置')
        self.ui.tabWidget.setTabText(1, '2.电机测试')
        self.ui.tabWidget.setCurrentIndex(0)

    def update_param(self, param):
        log.i("type:%d %d" % (param.model_type, param.imu_type))
        try:
            self.ui.combox_model.setCurrentIndex(self.model_index_list[param.model_type])
        except Exception as e:
            print("model type err")

        try:
            self.ui.combox_imu_type.setCurrentIndex(
                self.imu_type_list[self.imu_value_list[param.imu_type]])
        except Exception as e:
            print("imu type err")

        try:
            self.ui.slider_wheel_diameter.setSliderPosition(
                param.wheel_diameter)
            self.ui.slider_wheel_track.setSliderPosition(param.wheel_track)
            self.ui.slider_encoder.setSliderPosition(param.encoder_resolution)
            self.ui.slider_motor_ratio.setSliderPosition(param.motor_ratio)

            self.ui.checkBox_motor1.setChecked(
                param.motor_nonexchange_flag & 0x1)
            self.ui.checkBox_motor2.setChecked(
                param.motor_nonexchange_flag & 0x2)
            self.ui.checkBox_motor3.setChecked(
                param.motor_nonexchange_flag & 0x4)
            self.ui.checkBox_motor4.setChecked(
                param.motor_nonexchange_flag & 0x8)

            self.ui.checkBox_encoder1.setChecked(
                param.encoder_nonexchange_flag & 0x1)
            self.ui.checkBox_encoder2.setChecked(
                param.encoder_nonexchange_flag & 0x2)
            self.ui.checkBox_encoder3.setChecked(
                param.encoder_nonexchange_flag & 0x4)
            self.ui.checkBox_encoder4.setChecked(
                param.encoder_nonexchange_flag & 0x8)
        except Exception as e:
            print("motor dir param err")

        try:
            self.ui.slider_cmd_lasttime.setSliderPosition(param.cmd_last_time)
            self.ui.slider_vx_max.setSliderPosition(param.max_v_liner_x)
            self.ui.slider_vy_max.setSliderPosition(param.max_v_liner_y)
            self.ui.slider_va_max.setSliderPosition(param.max_v_angular_z)
        except Exception as e:
            print("pid param err")

        try:
            self.ui.slider_pid_interval.setSliderPosition(
                param.do_pid_interval)
            self.ui.slider_kp.setSliderPosition(param.kp)
            self.ui.slider_ki.setSliderPosition(param.ki)
            self.ui.slider_kd.setSliderPosition(param.kd)
            self.ui.slider_ko.setSliderPosition(param.ko)
        except Exception as e:
            print("velocity limit param err")

    def read_params(self):
        # get robot parameter
        robotParam = self.DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
        p = self.mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
        if p:
            log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d"
                     % (robotParam.param.model_type,
                        robotParam.param.wheel_diameter,
                        robotParam.param.wheel_track,
                        robotParam.param.encoder_resolution
                        ))

            log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d"
                     % (robotParam.param.do_pid_interval,
                        robotParam.param.kp,
                        robotParam.param.ki,
                        robotParam.param.kd,
                        robotParam.param.ko))

            log.info("cmd_last_time:%d imu_type:%d"
                     % (robotParam.param.cmd_last_time,
                        robotParam.param.imu_type
                        ))

            log.info("max_v:%d %d %d"
                     % (robotParam.param.max_v_liner_x,
                        robotParam.param.max_v_liner_y,
                        robotParam.param.max_v_angular_z
                        ))

            log.info("motor flag:%d encoder flag: %d"
                     % (robotParam.param.motor_nonexchange_flag,
                        robotParam.param.encoder_nonexchange_flag
                        ))
        else:
            log.error('get params err')
            return False

        self.update_param(robotParam.param)

    def get_input_param(self):
        params = dataholder.RobotParameters()

        params.wheel_diameter = self.ui.slider_wheel_diameter.sliderPosition()
        params.wheel_track = self.ui.slider_wheel_track.sliderPosition()
        params.encoder_resolution = self.ui.slider_encoder.sliderPosition()
        params.do_pid_interval = self.ui.slider_pid_interval.sliderPosition()
        params.kp = self.ui.slider_kp.sliderPosition()
        params.ki = self.ui.slider_ki.sliderPosition()
        params.kd = self.ui.slider_kd.sliderPosition()
        params.ko = self.ui.slider_ko.sliderPosition()
        params.cmd_last_time = self.ui.slider_cmd_lasttime.sliderPosition()
        params.max_v_liner_x = self.ui.slider_vx_max.sliderPosition()
        params.max_v_liner_y = self.ui.slider_vy_max.sliderPosition()
        params.max_v_angular_z = self.ui.slider_va_max.sliderPosition()
        params.motor_ratio = self.ui.slider_motor_ratio.sliderPosition()
        params.imu_type = self.imu_index_list[self.ui.combox_imu_type.currentIndex(
        )]

        params.model_type = self.model_value_list[self.ui.combox_model.currentIndex()]

        params.motor_nonexchange_flag = 0

        if self.ui.checkBox_motor1.isChecked():
            params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x1
        else:
            params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfe

        if self.ui.checkBox_motor2.isChecked():
            params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x2
        else:
            params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfd

        if self.ui.checkBox_motor3.isChecked():
            params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x4
        else:
            params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xfb

        if self.ui.checkBox_motor4.isChecked():
            params.motor_nonexchange_flag = params.motor_nonexchange_flag | 0x8
        else:
            params.motor_nonexchange_flag = params.motor_nonexchange_flag & 0xf7

        params.encoder_nonexchange_flag = 0

        if self.ui.checkBox_encoder1.isChecked():
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x1
        else:
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfe

        if self.ui.checkBox_encoder2.isChecked():
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x2
        else:
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfd

        if self.ui.checkBox_encoder3.isChecked():
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x4
        else:
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xfb

        if self.ui.checkBox_encoder4.isChecked():
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag | 0x8
        else:
            params.encoder_nonexchange_flag = params.encoder_nonexchange_flag & 0xf7

        return params

    def write_params(self):
        self.DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = self.get_input_param()
        p = self.mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
        if p:
            log.info('set parameter success')
        else:
            log.error('set parameter err')
            quit(1)
    
    def update_pid_debug(self, pid_data):
        self.ui.label_input_1.setText(str(pid_data[0]))
        self.ui.label_input_2.setText(str(pid_data[1]))
        self.ui.label_input_3.setText(str(pid_data[2]))
        self.ui.label_input_4.setText(str(pid_data[3]))
        self.ui.label_output_1.setText(str(pid_data[4]))
        self.ui.label_output_2.setText(str(pid_data[5]))
        self.ui.label_output_3.setText(str(pid_data[6]))
        self.ui.label_output_4.setText(str(pid_data[7]))

    def update_imu(self, imu):
        #log.info('imu: %s'%(('\t\t').join([str(x) for x in imu])))
        self.ui.label_acc_x.setText(str(round(imu[0], 6)))
        self.ui.label_acc_y.setText(str(round(imu[1], 6)))
        self.ui.label_acc_z.setText(str(round(imu[2], 6)))
        self.ui.label_gyro_x.setText(str(round(imu[3], 6)))
        self.ui.label_gyro_y.setText(str(round(imu[4], 6)))
        self.ui.label_gyro_z.setText(str(round(imu[5], 6)))
        self.ui.label_magn_x.setText(str(round(imu[6], 6)))
        self.ui.label_magn_y.setText(str(round(imu[7], 6)))
        self.ui.label_magn_z.setText(str(round(imu[8], 6)))

    def update_encoder(self, encoder):
        log.debug('encoder count: %s'%(('\t\t').join([str(x) for x in encoder])))
        self.ui.label_feedback1.setText(str(encoder[0]))
        self.ui.label_feedback2.setText(str(encoder[1]))
        self.ui.label_feedback3.setText(str(encoder[2]))
        self.ui.label_feedback4.setText(str(encoder[3]))


    def _read_encoder(self):
        while self._KeepRunning:
            robot_encoder = self.DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
            p = self.mboard.request(MessageID.ID_GET_ENCODER_COUNT)
            if p:
                # log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robot_encoder])))
                self.encoder_signal.emit([int(x) for x in robot_encoder])
            
            robot_imu = self.DataHolder[MessageID.ID_GET_IMU].imu
            p = self.mboard.request(MessageID.ID_GET_IMU)
            if p:
                # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu])))
                self.imu_signal.emit([x for x in robot_imu])

            pid_data = self.DataHolder[MessageID.ID_GET_PID_DEBUG].pid_data
            p = self.mboard.request(MessageID.ID_GET_PID_DEBUG)
            if p:
                # log.info('imu: %s'%(('\t\t').join([str(x) for x in robot_imu])))
                self.pid_debug_signal.emit([x for x in pid_data])
            import time
            time.sleep(0.1)

    def start_motor(self):
        self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [self.ui.slider_set_pwm1.sliderPosition(),
                                                            self.ui.slider_set_pwm2.sliderPosition(),
                                                            self.ui.slider_set_pwm3.sliderPosition(),
                                                            self.ui.slider_set_pwm4.sliderPosition()]
        p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM)
        if p:
            log.info('set pwm success')
        else:
            log.error('set pwm err')

    def stop_motor(self):
        self.DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
        p = self.mboard.request(MessageID.ID_SET_MOTOR_PWM)
        if p:
            log.info('set pwm success')
        else:
            log.error('set pwm err')
        
    def start_control(self):
        self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 200
        self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0
        self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 10
        p = self.mboard.request(MessageID.ID_SET_VEL)
        if p:
            log.info('set vel success')
        else:
            log.error('set vel err')

    def stop_control(self):
        self.DataHolder[MessageID.ID_SET_VEL].v_liner_x = 0
        self.DataHolder[MessageID.ID_SET_VEL].v_liner_y = 0
        self.DataHolder[MessageID.ID_SET_VEL].v_angular_z = 0
        p = self.mboard.request(MessageID.ID_SET_VEL)
        if p:
            log.info('set vel success')
        else:
            log.error('set vel err')
        
    def init_dev(self):
        self.mboard = Transport(port, params.pibotBaud)
        if not self.mboard.start():
            log.error("can not open %s" % port)
            return False

        self.DataHolder = self.mboard.getDataHolder()

        for num in range(0, 3):
            log.info("****************get robot version*****************")
            boardVersion = self.DataHolder[MessageID.ID_GET_VERSION]
            p = self.mboard.request(MessageID.ID_GET_VERSION)
            if p:
                log.info("firmware version:%s buildtime:%s" % (
                    boardVersion.version.decode(), boardVersion.build_time.decode()))
                break
            else:
                log.error('read firmware version err')
                import time
                time.sleep(1)
                if num == 2:
                    log.error('please check connection or baudrate')
                    return False

        return self.read_params()

if __name__ == '__main__':
    app = QApplication(sys.argv)

    import qdarkstyle
    app.setStyleSheet(qdarkstyle.load_stylesheet(qt_api='pyqt5'))

    # from qt_material import apply_stylesheet
    # apply_stylesheet(app, theme='light_teal.xml')
    myDlg = MainDialog()
    myDlg.show()
    sys.exit(app.exec_())