from PyQt5.QtWidgets import QWidget, QGridLayout, QLabel, QLineEdit, QSlider, \
    QPushButton, QButtonGroup, QCheckBox, QVBoxLayout
from PyQt5.QtCore import Qt, QTimer
from SetpointHandler import SetpointHandler, FlightMode, Setpoint
from cfclient.utils.input import JoystickReader
from GamepadWizard import DeviceReader
from cfclient.utils.config_manager import ConfigManager
from threading import Semaphore


class SetpointMenu(QWidget):

    def __init__(self, setpoint_handler: SetpointHandler,
                 joystick: JoystickReader, ):
        super().__init__()

        self.setpoint_handler = setpoint_handler
        self.joystick = joystick

        self.joystick_reader = DeviceReader(self.joystick)

        layout = QVBoxLayout()

        grid_widget = QWidget()
        grid_layout = QGridLayout()
        grid_widget.setLayout(grid_layout)

        layout.addWidget(grid_widget, 1)

        self.setLayout(layout)
        self.timer = QTimer()
        self.setpoint = Setpoint()
        self.setpoint_semaphore = Semaphore(1)

        # ------------------- Gamepad Selection --------------------------------

        self.gamepad_button = QCheckBox("Gamepad Mode")
        self.setpoint_button = QCheckBox("Setpoint Mode")
        self.setpoint_button.setChecked(True)
        self.gamepad_button.toggled.connect(self.toggleSetpointMode)
        self.setpoint_button.toggled.connect(self.toggleSetpointMode)

        self.gamepad_or_setpoint = QButtonGroup()
        self.gamepad_or_setpoint.addButton(self.gamepad_button, 1)
        self.gamepad_or_setpoint.addButton(self.setpoint_button, 2)
        grid_layout.addWidget(self.gamepad_button, 0, 0)
        grid_layout.addWidget(self.setpoint_button, 0, 1)

        # ------------------ Setpoints -----------------------------------------
        yaw_label = QLabel("Yaw Setpoint:")
        pitch_label = QLabel("Pitch Setpoint:")
        roll_label = QLabel("Roll Setpoint:")
        self.yaw_box = QLineEdit()
        self.pitch_box = QLineEdit()
        self.roll_box = QLineEdit()

        grid_layout.addWidget(yaw_label, 1, 0)
        grid_layout.addWidget(self.yaw_box, 1, 1)
        grid_layout.addWidget(pitch_label, 2, 0)
        grid_layout.addWidget(self.pitch_box, 2, 1)
        grid_layout.addWidget(roll_label, 3, 0)
        grid_layout.addWidget(self.roll_box, 3, 1)

        # ------------------ Thrust --------------------------------------------
        # Add the sliders and thrust label
        thrust_label = QLabel("Thrust:")
        self.thrust_slider = QSlider(Qt.Horizontal)
        self.thrust_slider.setMinimum(0)
        self.thrust_slider.setMaximum(100)

        grid_layout.addWidget(thrust_label, 4, 0)
        grid_layout.addWidget(self.thrust_slider, 4, 1)

        # ----------------- Attitude or Rate mode ------------------------------

        self.b1 = QCheckBox("Attitude Mode")
        self.b2 = QCheckBox("Rate Mode")
        self.b2.setChecked(True)
        self.b1.toggled.connect(self.toggle_flight_mode_while_running)
        self.b1.toggled.connect(self.toggle_flight_mode_while_running)

        self.attitude_or_rate = QButtonGroup()
        self.attitude_or_rate.addButton(self.b1, 1)
        self.attitude_or_rate.addButton(self.b2, 2)
        grid_layout.addWidget(self.b1, 5, 0)
        grid_layout.addWidget(self.b2, 5, 1)

        # ----------------- Send Setpoint / Stop Flying ------------------------

        self.valid_label = QLabel("Setpoint Valid")

        # Add the print button
        self.send_setpoint_button = QPushButton("Send Setpoint")
        self.send_setpoint_button.clicked.connect(self.send_setpoint)

        self.stop_flying_button = QPushButton("Stop Flying")
        self.stop_flying_button.clicked.connect(self.stop_flying)

        layout.addWidget(self.valid_label, 2)
        layout.addWidget(self.send_setpoint_button, 3)
        layout.addWidget(self.stop_flying_button, 4)

    def send_setpoint(self):
        print("Send setpoint")

        yaw = self.yaw_box.text()
        pitch = self.pitch_box.text()
        roll = self.roll_box.text()
        thrust = self.thrust_slider.value()
        # self.setpoint_handler.commander.start_flying()

        if self.b1.isChecked():
            attitude_mode = True
        else:
            attitude_mode = False

        print(f"{yaw}, {pitch}, {roll}, {thrust}, {attitude_mode}")

        all_valid = True
        try:
            yaw = float(yaw)
            pitch = float(pitch)
            roll = float(roll)
            thrust = int(thrust)
            self.valid_label.setText("Setpoint Valid")
        except ValueError:
            all_valid = False
            self.valid_label.setText("Setpoint Invalid")

        if all_valid:
            if self.setpoint_handler.getFlightMode() == FlightMode.TYPE_STOP:
                print("from stop")
                self.setpoint_handler.commander.start_flying()

                if self.b1.isChecked():
                    self.setpoint_handler.setAttitudeMode()
                else:
                    self.setpoint_handler.setRateMode()
            self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)


    def stop_flying(self):
        print("Stop flying")
        self.setpoint_handler.stopFlying()

    def toggle_flight_mode_while_running(self):
        if self.setpoint_handler.getFlightMode() != FlightMode.TYPE_STOP:
            if self.b1.isChecked():
                self.setpoint_handler.setAttitudeMode()
            else:
                self.setpoint_handler.setRateMode()

    def printGamepad(self, data):
        self.setpoint_semaphore.acquire()

        self.setpoint.yaw = data.yaw
        self.setpoint.pitch = data.pitch
        self.setpoint.roll = data.roll
        self.setpoint.thrust = data.thrust

        self.setpoint_semaphore.release()

    def enableGamepad(self, current_selection_name: str):
        print(current_selection_name)
        loaded_map = ConfigManager().get_config(current_selection_name)
        if loaded_map:
            self.joystick.set_raw_input_map(loaded_map)
            print("Joystick set")
        else:
            print("error loading config")

        self.joystick_reader.start_reading()
        self.joystick_reader.mapped_values_signal.connect(self.printGamepad)

    def toggleSetpointMode(self):
        if self.gamepad_button.isChecked():
            self.yaw_box.setEnabled(False)
            self.pitch_box.setEnabled(False)
            self.roll_box.setEnabled(False)
            self.thrust_slider.setEnabled(False)
            self.b1.setEnabled(False)
            self.b2.setEnabled(False)
            self.send_setpoint_button.setEnabled(False)
            self.stop_flying_button.setEnabled(False)

            # todo add callback for sending setpoint continuously

            self.timer.timeout.connect(self.sendGamepadSetpoint)
            self.timer.start(100)

        else:
            self.yaw_box.setEnabled(True)
            self.pitch_box.setEnabled(True)
            self.roll_box.setEnabled(True)
            self.thrust_slider.setEnabled(True)
            self.b1.setEnabled(True)
            self.b2.setEnabled(True)
            self.send_setpoint_button.setEnabled(True)
            self.stop_flying_button.setEnabled(True)

            self.timer.timeout.disconnect(self.sendGamepadSetpoint)
            self.timer.stop()

    def sendGamepadSetpoint(self):

        self.setpoint_semaphore.acquire()

        self.yaw_box.setText(str(round(self.setpoint.yaw, 2)))
        self.roll_box.setText(str(round(self.setpoint.roll, 2)))
        self.pitch_box.setText(str(round(self.setpoint.pitch, 2)))
        self.thrust_slider.setValue(int(round(self.setpoint.thrust, 2)))
        self.send_setpoint()

        self.setpoint_semaphore.release()