class CarlaControlPlugin(Plugin):
    """
    RQT Plugin to control CARLA
    """
    def __init__(self, context):
        """
        Constructor
        """
        super(CarlaControlPlugin, self).__init__(context)
        self.setObjectName('CARLA Control')

        self._widget = QWidget()

        self._node = CompatibleNode('rqt_carla_control_node')

        package_share_dir = roscomp.get_package_share_directory(
            'rqt_carla_control')
        ui_file = os.path.join(package_share_dir, 'resource',
                               'CarlaControl.ui')

        loadUi(ui_file, self._widget)
        self._widget.setObjectName('CarlaControl')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        self.pause_icon = QIcon(
            QPixmap(os.path.join(package_share_dir, 'resource', 'pause.png')))
        self.play_icon = QIcon(
            QPixmap(os.path.join(package_share_dir, 'resource', 'play.png')))
        self._widget.pushButtonStepOnce.setIcon(
            QIcon(
                QPixmap(
                    os.path.join(package_share_dir, 'resource',
                                 'step_once.png'))))

        self.carla_status = None
        self.carla_status_subscriber = self._node.new_subscription(
            CarlaStatus,
            "/carla/status",
            self.carla_status_changed,
            qos_profile=10)

        self.carla_control_publisher = self._node.new_publisher(
            CarlaControl, "/carla/control", qos_profile=10)

        self._widget.pushButtonPlayPause.setDisabled(True)
        self._widget.pushButtonStepOnce.setDisabled(True)
        self._widget.pushButtonPlayPause.setIcon(self.play_icon)
        self._widget.pushButtonPlayPause.clicked.connect(
            self.toggle_play_pause)
        self._widget.pushButtonStepOnce.clicked.connect(self.step_once)

        context.add_widget(self._widget)

        if roscomp.get_ros_version() == 2:
            spin_thread = threading.Thread(target=self._node.spin, daemon=True)
            spin_thread.start()

    def toggle_play_pause(self):
        """
        toggle play/pause
        """
        if self.carla_status.synchronous_mode:
            if self.carla_status.synchronous_mode_running:
                self.carla_control_publisher.publish(
                    CarlaControl(command=CarlaControl.PAUSE))
            else:
                self.carla_control_publisher.publish(
                    CarlaControl(command=CarlaControl.PLAY))

    def step_once(self):
        """
        execute one step
        """
        self.carla_control_publisher.publish(
            CarlaControl(command=CarlaControl.STEP_ONCE))

    def carla_status_changed(self, status):
        """
        callback whenever carla status changes
        """
        self.carla_status = status
        if status.synchronous_mode:
            self._widget.pushButtonPlayPause.setDisabled(False)
            self._widget.pushButtonStepOnce.setDisabled(False)
            if status.synchronous_mode_running:
                self._widget.pushButtonPlayPause.setIcon(self.pause_icon)
            else:
                self._widget.pushButtonPlayPause.setIcon(self.play_icon)
        else:
            self._widget.pushButtonPlayPause.setDisabled(True)
            self._widget.pushButtonStepOnce.setDisabled(True)

    def shutdown_plugin(self):
        """
        shutdown plugin
        """
        self._node.destroy_subscription(self.carla_control_publisher)
class RosVehicleControl(BasicControl):
    def __init__(self, actor, args=None):
        super(RosVehicleControl, self).__init__(actor)

        self._carla_actor = actor
        self._role_name = actor.attributes["role_name"]
        if not self._role_name:
            roscomp.logerr("Invalid role_name")

        self._path_topic_name = "waypoints"
        if "path_topic_name" in args:
            self._path_topic_name = args["path_topic_name"]

        roscomp.init("ros_agent_{}".format(self._role_name), args=None)

        self.node = CompatibleNode('ros_agent_{}'.format(self._role_name))

        self._current_target_speed = None
        self._current_path = None
        self.controller_launch = None

        self._target_speed_publisher = self.node.new_publisher(
            Float64, "/carla/{}/target_speed".format(self._role_name),
            QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
        self.node.loginfo(
            "Publishing target_speed on /carla/{}/target_speed".format(
                self._role_name))

        self._path_publisher = self.node.new_publisher(
            Path, "/carla/{}/{}".format(self._role_name,
                                        self._path_topic_name),
            QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
        self.node.loginfo("Publishing path on /carla/{}/{}".format(
            self._role_name, self._path_topic_name))

        if "launch" in args and "launch-package" in args:

            launch_file = args["launch"]
            launch_package = args["launch-package"]

            if ROS_VERSION == 1:
                executable = "roslaunch"
                cli_args = [launch_package, launch_file]
            elif ROS_VERSION == 2:
                executable = "ros2 launch"
                cli_args = [launch_package, launch_file + '.py']
            cli_args.append('role_name:={}'.format(self._role_name))

            # add additional launch parameters
            launch_parameters = []
            for key, value in args.items():
                if not key == "launch" and not key == "launch-package" and not key == "path_topic_name":
                    launch_parameters.append('{}:={}'.format(key, value))
                    cli_args.append('{}:={}'.format(key, value))

            self.node.loginfo(
                "{}: Launching {} from package {} (parameters: {})...".format(
                    self._role_name, launch_file, launch_package,
                    launch_parameters))

            cmdline = [executable] + cli_args
            self.controller_launch = ApplicationRunner(
                self.controller_runner_status_updated,
                self.controller_runner_log,
                'RosVehicleControl: launching controller node')
            self.controller_launch.execute(
                cmdline,
                env=os.environ,
            )

            self.node.loginfo(
                "{}: Successfully started ros vehicle control".format(
                    self._role_name))
        else:
            self.node.logwarn(
                "{}: Missing value for 'launch' and/or 'launch-package'.".
                format(self._role_name))

    def controller_runner_log(self, log):  # pylint: disable=no-self-use
        """
        Callback for application logs
        """
        self.node.logwarn("[Controller]{}".format(log))

    def controller_runner_status_updated(self, status):
        """
        Executed from application runner whenever the status changed
        """
        self.node.loginfo("Controller status is: {}".format(status))

    def update_target_speed(self, speed):
        super(RosVehicleControl, self).update_target_speed(speed)
        self.node.loginfo("{}: Target speed changed to {}".format(
            self._role_name, speed))
        self._target_speed_publisher.publish(Float64(data=speed))

    def update_waypoints(self, waypoints, start_time=None):
        super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
        self.node.loginfo("{}: Waypoints changed.".format(self._role_name))
        path = Path()
        path.header.stamp = roscomp.ros_timestamp(sec=self.node.get_time(),
                                                  from_sec=True)
        path.header.frame_id = "map"
        for wpt in waypoints:
            print(wpt)
            path.poses.append(
                PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
        self._path_publisher.publish(path)

    def reset(self):
        # set target speed to zero before closing as the controller can take time to shutdown
        self.update_target_speed(0.)
        if self.controller_launch and self.controller_launch.is_running():
            self.controller_launch.shutdown()
        if self._carla_actor and self._carla_actor.is_alive:
            self._carla_actor = None
        if self._target_speed_publisher:
            self.node.destroy_subscription(self._target_speed_publisher)
            self._target_speed_publisher = None
        if self._path_publisher:
            self.node.destroy_subscription(self._path_publisher)
            self._path_publisher = None

    def run_step(self):
        pass