"""Custom Behavior Tree Node Plugins (Issue #482)"""
import rclpy
from rclpy.node import Node
from py_trees import behaviour, common
from geometry_msgs.msg import Twist
from sensor_msgs.msg import BatteryState
from std_msgs.msg import Float32, Bool

class GetBatteryLevel(behaviour.Behaviour):
    def __init__(self, name, node: Node):
        super().__init__(name)
        self.node = node
        self.battery_level = 100.0
        self.subscription = node.create_subscription(BatteryState, '/battery_state', self._battery_callback, 10)
    def _battery_callback(self, msg):
        self.battery_level = msg.percentage * 100.0
    def update(self) -> common.Status:
        self.blackboard.set('battery_level', self.battery_level, overwrite=True)
        return common.Status.SUCCESS

class CheckBatteryLevel(behaviour.Behaviour):
    def __init__(self, name, node: Node, threshold: float = 50.0):
        super().__init__(name)
        self.node = node
        self.threshold = threshold
        self.battery_level = 100.0
        self.subscription = node.create_subscription(BatteryState, '/battery_state', self._battery_callback, 10)
    def _battery_callback(self, msg):
        self.battery_level = msg.percentage * 100.0
    def update(self) -> common.Status:
        return common.Status.SUCCESS if self.battery_level >= self.threshold else common.Status.FAILURE

class CheckPersonDetected(behaviour.Behaviour):
    def __init__(self, name, node: Node):
        super().__init__(name)
        self.node = node
        self.person_detected = False
        self.subscription = node.create_subscription(Bool, '/person_detection/detected', self._detection_callback, 10)
    def _detection_callback(self, msg):
        self.person_detected = msg.data
    def update(self) -> common.Status:
        self.blackboard.set('person_detected', self.person_detected, overwrite=True)
        return common.Status.SUCCESS if self.person_detected else common.Status.FAILURE

class StopRobot(behaviour.Behaviour):
    def __init__(self, name, node: Node):
        super().__init__(name)
        self.node = node
        self.cmd_vel_pub = node.create_publisher(Twist, '/cmd_vel', 10)
    def update(self) -> common.Status:
        msg = Twist()
        self.cmd_vel_pub.publish(msg)
        return common.Status.SUCCESS

class SetBlackboardVariable(behaviour.Behaviour):
    def __init__(self, name, node: Node, var_name: str, var_value):
        super().__init__(name)
        self.node = node
        self.var_name = var_name
        self.var_value = var_value
    def update(self) -> common.Status:
        self.blackboard.set(self.var_name, self.var_value, overwrite=True)
        return common.Status.SUCCESS
